Фильтр частиц является основанным на выборке рекурсивным Байесовым алгоритмом оценки, который реализован в stateEstimatorPF
объект. В этом примере подобный автомобилю робот с дистанционным управлением прослеживается в наружной среде. Измерение положения робота обеспечивается встроенным GPS, который является шумным. Существуют известные команды движения, отправленные в робота, но робот не выполнит точное движение, которым управляют, из-за механической слабой или погрешности модели. Этот пример покажет, как использовать stateEstimatorPF
уменьшать эффекты шума в данных об измерении и получить более точную оценку положения робота. Кинематическая модель подобного автомобилю робота описана следующей нелинейной системой. Фильтр частиц идеально подходит для оценки состояния такого вида систем, когда это может иметь дело со свойственной нелинейностью.
Сценарий: подобный автомобилю робот управляет и изменяет свою скорость и держащийся угол постоянно. Положение робота измеряется некоторой шумной внешней системой, e.g. GPS или система Vicon. Вдоль пути это проедет область с крышей, где никакое измерение не может быть сделано.
Входной параметр:
Шумное измерение на частичном положении робота (, , ). Обратите внимание, что это не полное измерение состояния. Никакое измерение не доступно на передней ориентации колеса () а также все скорости (, , , ).
Линейные и угловые скорости отправляются в робота (, ). Примечание там будет некоторым различием между движением, которым управляют, и фактическим движением робота.
Цель: Оцените частичное положение (, , ) из подобного автомобилю робота. Обратите внимание снова что ориентация колеса () не включен в оценку. С точки зрения наблюдателя полное состояние автомобиля только [ , , , , , ].
Подход: Использование stateEstimatorPF
чтобы обработать два шумных входных параметров (ни одни из входных параметров не надежны отдельно) и делают лучшую оценку текущего (частичного) положения.
На предсказать этапе мы обновляем состояния частиц с упрощенной, подобной одноколесному велосипеду моделью робота, как показано ниже. Обратите внимание на то, что системная модель, используемая для оценки состояния, не является точным представлением фактической системы. Это приемлемо, пока различие модели хорошо получено в системном шуме (как представлено роем частицы). Для получения дополнительной информации смотрите predict
.
На правильном этапе вес важности (вероятность) частицы определяется ее ошибочной нормой из текущего измерения (), когда у нас только есть измерение на этих трех компонентах. Для получения дополнительной информации смотрите correct
.
rng('default'); % for repeatable result dt = 0.05; % time step initialPose = [0 0 0 0]'; carbot = ExampleHelperCarBot(initialPose, dt);
Этот раздел конфигурирует фильтр частиц с помощью 5 000 частиц. Первоначально все частицы случайным образом выбраны от нормального распределения со средним значением в модульной ковариации и начальном состоянии. Каждая частица содержит 6 переменных состояния (, , , , , ). Обратите внимание на то, что третья переменная отмечена как Проспект, поскольку это - автомобильная ориентация. Также очень важно задать две функции обратного вызова 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