stateEstimatorPF

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

Описание

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

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

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

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

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

Создание

Синтаксис

Описание

пример

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

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

Свойства

развернуть все

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

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

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

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

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

function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)

Функция обратного вызова принимает по крайней мере два входных параметра: stateEstimatorPF объект, pf, и частицы на предыдущем временном шаге, prevParticles. Этими заданными частицами является predictParticles возвращенный в предыдущий вызов объекта. predictParticles и prevParticles одного размера: NumParticles- 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- 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- NumStateVariables матрица. Каждая строка соответствует гипотезе состояния одной частицы.

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

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

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

Введенный в R2016a