Инициализируйте подобного Автомобилю робота

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

Настройте Фильтр частиц

Этот раздел конфигурирует фильтр частиц с помощью 5 000 частиц. Первоначально все частицы случайным образом выбраны от нормального распределения со средним значением в модульной ковариации и начальном состоянии. Каждая частица содержит 6 переменных состояния ($$x$$, $$y$$, $$\theta$$, $$\underset{}{\overset{\dot{}}{x}}$$, $$\underset{}{\overset{\dot{}}{y}}$$, $$\underset{}{\overset{\dot{}}{\theta }}$$). Обратите внимание на то, что третья переменная отмечена как Проспект, поскольку это - автомобильная ориентация. Также очень важно задать две функции обратного вызова 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, 2006