stateEstimatorPF

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

Описание

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

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

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

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

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

Создание

Синтаксис

pf = stateEstimatorPF

Описание

пример

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

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

Свойства

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

`NumStateVariables` - Количество переменных состояния
3 (по умолчанию) | скаляром

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

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

`NumParticles` - Количество частиц, используемых в фильтре
1000 (по умолчанию) | скаляром

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

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

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

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

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)

`MeasurementLikelihoodFcn` - Функция обратного вызова, вычисляющая вероятность измерений датчика
указатель на функцию

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

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

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

pf - Связанная stateEstimatorPF объект
predictParticles - частицы, которые представляют предсказанное состояние системы на текущем временном шаге как массив размера NumParticles-by- NumStateVariables
measurement - Измерение состояния на текущем временном шаге

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

correct(pf,measurement,arg1,arg2)

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

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

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

`IsStateVariableCircular` - Индикатор, если переменные состояния имеют круговое распределение
[0 0 0] (по умолчанию) | логический массив

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

`ResamplingPolicy` - Параметры политики, которые определяют, когда запускать повторную дискретизацию
объект

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

`ResamplingMethod` - Метод, используемый для повторной дискретизации частиц
`'multinomial'` (по умолчанию) | `'residual'` | `'stratified'` | `'systematic'`

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

`StateEstimationMethod` - Метод, используемый для оценки состояния
`'mean'` (по умолчанию) | `'maxweight'`

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

`Particles` - Массив значений частиц
`NumParticles`-by- `NumStateVariables` матрица

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

`Weights` - Веса частиц
`NumParticles`-by-1 вектор

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

`State` - Оценка наилучшего состояния
вектор

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

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

`State Covariance` - Исправленная система ковариации
N -by- N матрица `| []`

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

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

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

`initialize`	Инициализируйте состояние фильтра частиц
`getStateEstimate`	Извлеките лучшую оценку состояния и ковариацию из частиц
`predict`	Предсказать состояние робота на следующем временном шаге
`correct`	Скорректируйте оценку состояния на основе измерения датчика

Примеры

свернуть все

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

Открыть Live Script

Создайте 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

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

Открыть Live Script

Используйте 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.

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

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

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