Инициализируйте робота, похожего на автомобиль

rng('default'); % for repeatable result
dt = 0.05; % time step
initialPose = [0  0  0  0]';
carbot = ExampleHelperCarBot(initialPose, dt);

Настройка фильтра частиц

Этот раздел конфигурирует фильтр частиц, используя 5000 частиц. Первоначально все частицы случайным образом отбираются из нормального распределения со средним значением в начальном состоянии и единичной ковариацией. Каждая частица содержит 6 переменных состояния ($$x$$, $$y$$, $$\theta$$, $$\underset{}{\overset{\dot{}}{x}}$$, $$\underset{}{\overset{\dot{}}{y}}$$, $$\underset{}{\overset{\dot{}}{\theta }}$$). Обратите внимание, что третья переменная отмечена как Circular, поскольку это ориентация автомобиля. Также очень важно задать две функции обратного вызова StateTransitionFcn и MeasurementLikelihoodFcn. Эти две функции непосредственно определяют эффективность фильтра частиц. Детали этих двух функций можно найти в последних двух разделах этого примера.

pf = stateEstimatorPF;

initialize(pf, 5000, [initialPose(1:3)', 0, 0, 0], eye(6), 'CircularVariables',[0 0 1 0 0 0]);
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

% StateTransitionFcn defines how particles evolve without measurement
pf.StateTransitionFcn = @exampleHelperCarBotStateTransition;

% MeasurementLikelihoodFcn defines how measurement affect the our estimation
pf.MeasurementLikelihoodFcn = @exampleHelperCarBotMeasurementLikelihood;

% Last best estimation for x, y and theta
lastBestGuess = [0 0 0];

Основной цикл

Обратите внимание, что в этом примере командные линейные и угловые скорости для робота являются произвольно выбранными зависящими от времени функциями. Кроме того, обратите внимание, что синхронизация с фиксированной скоростью цикла реализуется через rateControl.

Запуск цикла с частотой 20 Гц в течение 20 секунд с помощью поддержки с фиксированной скоростью.

r = rateControl(1/dt);

Сбросьте объект с фиксированной скоростью, чтобы перезапустить таймер. Сбросьте таймер непосредственно перед запуском зависящего от времени кода.

reset(r);

simulationTime = 0; 

while simulationTime < 20 % if time is not up

    % Generate motion command that is to be sent to the robot
    % NOTE there will be some discrepancy between the commanded motion and the
    % motion actually executed by the robot. 
    uCmd(1) = 0.7*abs(sin(simulationTime)) + 0.1;  % linear velocity
    uCmd(2) = 0.08*cos(simulationTime);            % angular velocity
    
    drive(carbot, uCmd);
        
    % Predict the carbot pose based on the motion model
    [statePred, covPred] = predict(pf, dt, uCmd);

    % Get GPS reading
    measurement = exampleHelperCarBotGetGPSReading(carbot);

    % If measurement is available, then call correct, otherwise just use
    % predicted result
    if ~isempty(measurement)
        [stateCorrected, covCorrected] = correct(pf, measurement');
    else
        stateCorrected = statePred;
        covCorrected = covPred;
    end

    lastBestGuess = stateCorrected(1:3);

    % Update plot
    if ~isempty(get(groot,'CurrentFigure')) % if figure is not prematurely killed
        updatePlot(carbot, pf, lastBestGuess, simulationTime);
    else
        break
    end

    waitfor(r);
    
    % Update simulation time
    simulationTime = simulationTime + dt;
end

Подробная информация о результатах Фигур

Три рисунка показывают отслеживающую эффективность фильтра частиц.

Функция перехода к состоянию

Основанная на дискретизации функция перехода состояния развивает частицы на основе предписанной модели движения, так что частицы образуют представление распределения предложения. Ниже приведен пример переходной функции состояния, основанной на модели скоростного движения одноколесного робота. Для получения дополнительной информации об этой модели движения, пожалуйста, смотрите главу 5 в [1]. Уменьшите sd1, sd2 и sd3 чтобы увидеть, как ухудшается эффективность отслеживания. Вот sd1 представляет неопределенность в линейной скорости, sd2 представляет неопределенность в скорость вращения. sd3 является дополнительным возмущением ориентации.

   function predictParticles = exampleHelperCarBotStateTransition(pf, prevParticles, dT, u)

       thetas = prevParticles(:,3);

       w = u(2);
       v = u(1);

       l = length(prevParticles);

       % Generate velocity samples
       sd1 = 0.3;
       sd2 = 1.5;
       sd3 = 0.02;
       vh = v + (sd1)^2*randn(l,1);  
       wh = w + (sd2)^2*randn(l,1); 
       gamma = (sd3)^2*randn(l,1); 

       % Add a small number to prevent div/0 error
       wh(abs(wh)<1e-19) = 1e-19;

       % Convert velocity samples to pose samples
       predictParticles(:,1) = prevParticles(:,1) - (vh./wh).*sin(thetas) + (vh./wh).*sin(thetas + wh*dT);
       predictParticles(:,2) = prevParticles(:,2) + (vh./wh).*cos(thetas) - (vh./wh).*cos(thetas + wh*dT);
       predictParticles(:,3) = prevParticles(:,3) + wh*dT + gamma*dT;
       predictParticles(:,4) = (- (vh./wh).*sin(thetas) + (vh./wh).*sin(thetas + wh*dT))/dT;
       predictParticles(:,5) = ( (vh./wh).*cos(thetas) - (vh./wh).*cos(thetas + wh*dT))/dT;
       predictParticles(:,6) = wh + gamma;

   end

Функция правдоподобия измерения

Функция правдоподобия измерения вычисляет вероятность для каждой предсказанной частицы на основе нормы ошибки между частицей и измерением. Вес важности для каждой частицы будет назначаться на основе вычисленной вероятности. В этом конкретном примере predictParticles является матрицей N x 6 (N - количество частиц), и measurement является вектором 1 x 3.

   function  likelihood = exampleHelperCarBotMeasurementLikelihood(pf, predictParticles, measurement)

       % The measurement contains all state variables
       predictMeasurement = predictParticles;

       % Calculate observed error between predicted and actual measurement
       % NOTE in this example, we don't have full state observation, but only
       % the measurement of current pose, therefore the measurementErrorNorm
       % is only based on the pose error.
       measurementError = bsxfun(@minus, predictMeasurement(:,1:3), measurement);
       measurementErrorNorm = sqrt(sum(measurementError.^2, 2));

       % Normal-distributed noise of measurement
       % Assuming measurements on all three pose components have the same error distribution 
       measurementNoise = eye(3);

       % Convert error norms into likelihood measure. 
       % Evaluate the PDF of the multivariate normal distribution 
       likelihood = 1/sqrt((2*pi).^3 * det(measurementNoise)) * exp(-0.5 * measurementErrorNorm);

   end

Ссылка

[1] С. Трун, У. Бургард, Д. Фокс, Вероятностная робототехника, MIT Press, 2006