exponenta event banner

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вектор -by-1. Каждый вес связан с частицей в одном ряду в Particles собственность.

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

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

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

Скорректированная системная дисперсия, возвращаемая в виде матрицы N-на-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] Чен, З. «Байесовская фильтрация: от фильтров Калмана до фильтров частиц и далее». Статистика. т. 182, № 1, 2003, стр. 1-69.

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

Создание кода C/C + +
Создайте код C и C++ с помощью MATLAB ® Coder™

.
Представлен в R2016a