Рабочий процесс фильтра частиц

particle filter является рекурсивным, Байесовым средством оценки состояния, которое использует дискретные частицы, чтобы аппроксимировать апостериорное распределение предполагаемого состояния.

Алгоритм фильтра частиц вычисляет оценку состояния рекурсивно и включает два шага:

Прогноз – алгоритм использует предыдущее состояние, чтобы предсказать текущее состояние на основе данной системной модели.
Исправление – алгоритм использует измерение датчика тока, чтобы исправить оценку состояния.

Алгоритм также периодически перераспределяет или передискретизирует, частицы в пространстве состояний, чтобы совпадать с апостериорным распределением предполагаемого состояния.

Предполагаемое состояние состоит из всех переменных состояния. Каждая частица представляет гипотезу дискретного состояния. Набор всех частиц используется, чтобы помочь определить оценку конечного состояния.

Можно применить фильтр частиц к произвольным нелинейным системным моделям. Процесс и шум измерения могут следовать за произвольными нераспределениями Гаусса.

Чтобы использовать фильтр частиц правильно, необходимо задать параметры, такие как количество частиц, начального местоположения частицы и метода оценки состояния. Кроме того, если у вас есть определенное движение и модель датчика, вы задаете эти параметры в функции изменения состояния и функции правдоподобия измерения, соответственно. Для получения дополнительной информации смотрите Параметры Фильтра частиц.

Следуйте за этим основным рабочим процессом, чтобы создать и использовать фильтр частиц. Эта страница детализирует рабочий процесс оценки и показывает пример того, как запустить фильтр частиц в цикле, чтобы постоянно оценить состояние.

Рабочий процесс оценки

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

Создайте фильтр частиц

Создайте объект ParticleFilter путем вызова robotics.ParticleFilter.

Установите параметры нелинейной системы

Измените эти параметры ParticleFilter к пригодному для вашей определенной системы или приложения:

StateTransitionFcn
MeasurementLikelihoodFcn
ResamplingPolicy
ResamplingMethod
StateEstimationMethod

Значения по умолчанию для этих параметров даны для основной операции.

StateTransitionFcn и функции MeasurementLikelihoodFcn задают интегрирование измерения и поведение системы. Они жизненно важны для фильтра частиц, чтобы отследить точно. Для получения дополнительной информации смотрите Параметры Фильтра частиц.

Инициализируйте частицы

Используйте метод initialize, чтобы определить номер частиц и начального состояния. Смотрите robotics.ParticleFilter.initialize.

Демонстрационные частицы от распределения

Можно выбрать начальные местоположения частицы двумя способами:

Начальное положение и ковариация — Если у вас есть идея вашего начального состояния, рекомендуется, чтобы вы задали начальное положение и ковариацию. Эта спецификация помогает кластеризироваться, частицы ближе к вашей оценке, настолько отслеживающей, более вступает в силу с запуска.
Границы состояния — Если вы не знаете свое начальное состояние, можно задать возможные пределы каждой переменной состояния. Частицы равномерно распределены через границы состояния для каждой переменной. Широко распределенные частицы не являются столь же эффективными при отслеживании, потому что меньше частиц около реального положения. Используя границы состояния обычно требует, чтобы больше частиц, время вычисления и итерации сходилось к оценке реального положения.

Предсказать

На основе заданной функции изменения состояния частицы развиваются, чтобы оценить следующее состояние. Используйте predict, чтобы выполнить функцию изменения состояния, заданную в свойстве StateTransitionFcn. Смотрите robotics.ParticleFilter.predict.

Получите измерение

Измерения, собранные из датчиков, используются на следующем шаге, чтобы исправить текущее предсказанное состояние.

Правильный

Измерения затем используются, чтобы настроить предсказанное состояние и исправить оценку. Задайте свои измерения с помощью функции correct. robotics.ParticleFilter.correct использует MeasurementLikelihoodFcn, чтобы вычислить вероятность измерений датчика для каждой частицы. Передискретизация частиц требуется, чтобы обновлять вашу оценку как изменения состояния в последующих итерациях. Эта триггерная передискретизация шага на основе свойств ResamplingMethod и ResamplingPolicy.

Извлеките лучшую оценку состояния

После вызова correct лучшая оценка состояния автоматически извлечена на основе Weights каждой частицы и свойства StateEstimationMethod, заданного в robotics.ParticleFilter. Лучшее предполагаемое состояние и ковариация выводятся функцией correct.

Передискретизируйте частицы

Этот шаг отдельно не называется, но выполняется, когда вы вызываете correct. Если ваше состояние изменилось достаточно, передискретизируйте свои частицы на основе новейшей оценки. Метод correct проверяет ResamplingPolicy на инициирование передискретизации частицы согласно распределению тока частиц и их весов. Если передискретизация не инициирована, те же частицы используются для следующей оценки. Если ваше состояние не отличается очень или если ваш временной шаг является низким, можно вызвать предсказывание и исправить методы без передискретизации.

Постоянно предсказывайте и исправляйте

Повторите предыдущий прогноз, и исправление продвигается по мере необходимости для оценки состояния. Шаг исправления определяет, требуется ли передискретизация частиц. Множественные вызовы predict или correct могут требоваться когда:

Никакое измерение не доступно, но входные параметры управления, и обновления времени, происходят в высокой частоте. Используйте метод predict, чтобы развить частицы, чтобы получать обновленное предсказанное состояние чаще.
Несколько чтение измерения доступны. Используйте correct, чтобы интегрировать несколько показаний от того же самого или несколько датчиков. Функция исправляет состояние на основе каждого набора собранной информации.

Оцените положение робота в цикле Используя фильтр частиц

Открыть скрипт

Используйте объект ParticleFilter отследить робота, когда он перемещается в 2D пробел. Измеренное положение имеет случайный добавленный шум. Используя predict и correct, отследите робота на основе измерения и на принятой модели движения.

Инициализируйте фильтр частиц и задайте функцию перехода состояния по умолчанию, функцию правдоподобия измерения и политику передискретизации.

pf = robotics.ParticleFilter;
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

Демонстрационные 1 000 частиц с исходным положением [0 0] и модульная ковариация.

initialize(pf,1000,[0 0],eye(2));

До оценки задайте путь к синусоиде для точки, чтобы следовать. Создайте массив, чтобы сохранить предсказанное и предполагаемое положение. Задайте амплитуду шума.

t = 0:0.1:4*pi;
dot = [t; sin(t)]';
robotPred = zeros(length(t),2);
robotCorrected = zeros(length(t),2);
noise = 0.1;

Начните цикл для предсказания и исправления предполагаемого положения на основе измерений. Передискретизация частиц происходит на основе свойства ResamplingPolicy. Робот перемещается на основе функции синусоиды со случайным шумом, добавленным к измерению.

for i = 1:length(t)
    % Predict next position. Resample particles if necessary.
    [robotPred(i,:),robotCov] = predict(pf);
    % Generate dot measurement with random noise. This is
    % equivalent to the observation step.
    measurement(i,:) = dot(i,:) + noise*(rand([1 2])-noise/2);
    % Correct position based on the given measurement to get best estimation.
    % Actual dot position is not used. Store corrected position in data array.
    [robotCorrected(i,:),robotCov] = correct(pf,measurement(i,:));
end

Постройте фактический путь по сравнению с предполагаемым положением. Фактические результаты могут отличаться из-за случайности дистрибутивов частицы.

plot(dot(:,1),dot(:,2),robotCorrected(:,1),robotCorrected(:,2),'or')
xlim([0 t(end)])
ylim([-1 1])
legend('Actual position','Estimated position')
grid on

Данные показывают, как близко оценочное состояние совпадает с фактическим положением робота. Попытайтесь настроить количество частиц или задать различное исходное положение и ковариацию, чтобы видеть, как это влияет на отслеживание в зависимости от времени.

Документация