Создайте средство оценки состояния фильтра частиц
stateEstimatorPF объект является рекурсивным, Байесовым средством оценки состояния, которое использует дискретные частицы, чтобы аппроксимировать апостериорное распределение предполагаемого состояния.
Алгоритм фильтра частиц вычисляет оценку состояния рекурсивно и включает два шага: прогноз и коррекция. Шаг прогноза использует предыдущее состояние, чтобы предсказать текущее состояние на основе данной системной модели. Шаг коррекции использует измерение датчика тока, чтобы откорректировать оценку состояния. Алгоритм периодически перераспределяет или передискретизирует, частицы в пространстве состояний, чтобы совпадать с апостериорным распределением предполагаемого состояния.
Предполагаемое состояние состоит из переменных состояния. Каждая частица представляет гипотезу дискретного состояния этих переменных состояния. Набор всех частиц используется, чтобы помочь определить оценку конечного состояния.
Можно применить фильтр частиц к произвольным нелинейным системным моделям. Процесс и шум измерения могут следовать за произвольными нераспределениями Гаусса.
Для получения дополнительной информации о рабочем процессе фильтра частиц и установке определенных параметров, см.:
pf = stateEstimatorPFinitialize метод, чтобы инициализировать частицы с известным средним значением и ковариацией или равномерно распределенные частицы в заданных границах. Чтобы настроить систему фильтра частиц и модели измерения, измените StateTransitionFcn и MeasurementLikelihoodFcn свойства.
После того, как вы создадите объект, используйте initialize инициализировать NumStateVariables и NumParticles свойства. initialize функционируйте устанавливает эти два свойства на основе ваших входных параметров.
initialize | Инициализируйте состояние фильтра частиц |
getStateEstimate | Извлечение лучше всего утверждает оценку и ковариацию от частиц |
predict | Предскажите состояние робота в следующем временном шаге |
correct | Настройте оценку состояния на основе измерения датчика |
Создайте stateEstimatorPF объект, и выполняет прогноз и шаг коррекции для оценки состояния. Фильтр частиц дает предсказанную оценку состояния на основе возвращаемого значения StateTransitionFcn. Это затем корректирует состояние на основе данного измерения и возвращаемого значения MeasurementLikelihoodFcn.
Создайте фильтр частиц со значением по умолчанию три состояния.
pf = stateEstimatorPF
pf =
stateEstimatorPF with properties:
NumStateVariables: 3
NumParticles: 1000
StateTransitionFcn: @nav.algs.gaussianMotion
MeasurementLikelihoodFcn: @nav.algs.fullStateMeasurement
IsStateVariableCircular: [0 0 0]
ResamplingPolicy: [1x1 resamplingPolicyPF]
ResamplingMethod: 'multinomial'
StateEstimationMethod: 'mean'
StateOrientation: 'row'
Particles: [1000x3 double]
Weights: [1000x1 double]
State: 'Use the getStateEstimate function to see the value.'
StateCovariance: 'Use the getStateEstimate function to see the value.'
Задайте средний метод оценки состояния и систематический метод передискретизации.
pf.StateEstimationMethod = 'mean'; pf.ResamplingMethod = 'systematic';
Инициализируйте фильтр частиц в состоянии [4 1 9] с модульной ковариацией (eye(3)). Используйте 5 000 частиц.
initialize(pf,5000,[4 1 9],eye(3));
При принятии измерения [4.2 0.9 9], запуститесь, каждый предсказывает и один правильный шаг.
[statePredicted,stateCov] = predict(pf); [stateCorrected,stateCov] = correct(pf,[4.2 0.9 9]);
Получите лучшую оценку состояния на основе StateEstimationMethod алгоритм.
stateEst = getStateEstimate(pf)
stateEst = 1×3
4.1562 0.9185 9.0202
Используйте stateEstimatorPF возразите, чтобы отследить робота, когда он перемещается в 2D пробел. Измеренное положение имеет случайный добавленный шум. Используя predict и correct, отследите робота на основе измерения и на принятой модели движения.
Инициализируйте фильтр частиц и задайте функцию перехода состояния по умолчанию, функцию правдоподобия измерения и политику передискретизации.
pf = stateEstimatorPF; 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;
Начните цикл для предсказания и исправления предполагаемого положения на основе измерений. Передискретизация частиц происходит на основе theResamplingPolicy свойство. Робот перемещается на основе функции синусоиды со случайным шумом, добавленным к измерению.
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
Рисунок показывает, как близко оценочное состояние совпадает с фактическим положением робота. Попытайтесь настроить количество частиц или задать различное исходное положение и ковариацию, чтобы видеть, как это влияет на отслеживание в зависимости от времени.
[1] Arulampalam, M.S., С. Мэскелл, Н. Гордон и Т. Клэпп. "Пример на Фильтрах частиц для Онлайнового Байесового Отслеживания Nonlinear/Non-Gaussian". Транзакции IEEE на Обработке сигналов. Издание 50, № 2, февраль 2002, стр 174-188.
[2] Чен, Z. "Байесова Фильтрация: От Фильтров Калмана до Фильтров частиц, и Вне". Статистика. Издание 182, № 1, 2003, стр 1-69.