stateEstimatorPF

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

Описание

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

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

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

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

Для получения дополнительной информации о рабочем процессе фильтра частиц и установке конкретных параметров смотрите:

Создание

Синтаксис

Описание

пример

pf = stateEstimatorPF создает объект, который включает оценку состояния для простой системы с тремя переменными состояния. Используйте initialize способ инициализации частиц с известными средними и ковариационными или равномерно распределенными частицами в определенных границах. Чтобы настроить систему фильтра частиц и модели измерения, измените StateTransitionFcn и MeasurementLikelihoodFcn свойства.

После создания объекта используйте initialize чтобы инициализировать NumStateVariables и NumParticles свойства. The initialize функция устанавливает эти два свойства на основе ваших входов.

Свойства

расширить все

Это свойство доступно только для чтения.

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

Это свойство доступно только для чтения.

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

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

function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)

Функция обратного вызова принимает как минимум два входных параметров: stateEstimatorPF объект, pfи частицы на предыдущем временном шаге, prevParticles. Эти заданные частицы являются predictParticles возвращен из предыдущего вызова объекта. predictParticles и prevParticles одинаковые размеры: NumParticles-by- NumStateVariables.

Можно также использовать varargin для передачи в переменном количестве аргументов из predict функция. Когда вы звоните:

predict(pf,arg1,arg2)

MATLAB® по существу вызывает stateTranstionFcn как:

stateTransitionFcn(pf,prevParticles,arg1,arg2)

Функция обратного вызова, вычисляющая вероятность измерений датчика, заданная как указатель на функцию. Когда измерение датчика доступно, эта функция обратного вызова вычисляет вероятность того, что измерение соответствует гипотезе о состоянии каждой частицы. Вы должны реализовать эту функцию на основе вашей модели измерения. Сигнатура функции:

function likelihood = measurementLikelihoodFcn(PF,predictParticles,measurement,varargin)

Функция обратного вызова принимает по крайней мере три входных параметров:

  1. pf - Связанная stateEstimatorPF объект

  2. predictParticles - частицы, которые представляют предсказанное состояние системы на текущем временном шаге как массив размера NumParticles-by- NumStateVariables

  3. measurement - Измерение состояния на текущем временном шаге

Можно также использовать varargin для передачи переменного количества аргументов. Эти аргументы передаются correct функция. Когда вы звоните:

correct(pf,measurement,arg1,arg2)

MATLAB по сути вызывает measurementLikelihoodFcn как:

measurementLikelihoodFcn(pf,predictParticles,measurement,arg1,arg2)

Коллбэк должен вернуть только один выход, likelihood, что является вероятностью данного measurement для каждой гипотезы о состоянии частиц.

Индикатор, имеют ли переменные состояния круговое распределение, заданное как логический массив. Округлые (или угловые) распределения используют функцию плотности вероятностей с областью значений [-pi,pi]. Если объект имеет несколько переменных состояния, то IsStateVariableCircular является вектор-строка. Каждый векторный элемент указывает, является ли связанная переменная состояния округлой. Если у объекта только одна переменная состояния, то IsStateVariableCircular является скаляром.

Параметры политики, определяющие, когда запускать повторную дискретизацию, заданные как объект. Можно инициировать повторную дискретизацию или через фиксированные интервалы, или можно инициировать ее динамически, основываясь на количестве эффективных частиц. См. resamplingPolicyPF для получения дополнительной информации.

Метод, используемый для повторной дискретизации частиц, указанный как 'multinomial', 'residual', 'stratified', и 'systematic'.

Метод, используемый для оценки состояния, задается как 'mean' и 'maxweight'.

Массив значений частиц, заданный как NumParticles-by- NumStateVariables матрица. Каждая строка соответствует гипотезе о состоянии одной частицы.

Веса частиц, заданные как NumParticles-by-1 вектор. Каждый вес связан с частицей в одной строке в Particles свойство.

Это свойство доступно только для чтения.

Лучшая оценка состояния, возвращенная как вектор с NumStateVariables длины. Оценка извлекается на основе StateEstimationMethod свойство.

Это свойство доступно только для чтения.

Скорректированное системное отклонение, возвращенная как N -by - N матрица, где N равно NumStateVariables свойство. Исправленное состояние вычисляется на основе StateEstimationMethod свойство и MeasurementLikelihoodFcn. Если вы задаете метод оценки состояния, который не поддерживает ковариацию, то свойство устанавливается на [].

Функции объекта

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)). Используйте 5000 частицы.

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 объект для отслеживания робота, когда он перемещается в 2-D пространстве. Измеренное положение имеет добавленный случайный шум. Использование predict и correct, отслеживайте робота на основе измерения и на предполагаемой модели движения.

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

pf = stateEstimatorPF;
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

Выборка 1000 частицы с начальным положением [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

Figure contains an axes. The axes contains 2 objects of type line. These objects represent Actual position, Estimated position.

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

Вопросы совместимости

расширить все

Изменение поведения в будущем релизе

Ссылки

[1] Арулампалам, М. С., С. Маскелл, Н. Гордон и Т. Клэпп. Руководство по фильтрам частиц для онлайн-нелинейного/негауссовского байесовского отслеживания. Транзакции IEEE по обработке сигналов. Том 50, № 2, фев 2002, с. 174-188.

[2] Chen, Z. «Bayesian Filtering: From Kalman Filters to Фильтры Частиц, and Beyond». Статистика. Том 182, № 1, 2003, стр. 1-69.

Расширенные возможности

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ MATLAB ®

.
Введенный в R2016a