Exponenta Banner

робототехника. Класс ParticleFilter

Пакет: робототехника

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

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

Описание

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

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

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

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

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

Конструкция

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

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

Свойства

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

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

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

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

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

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

function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)

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

  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]. Если объект ParticleFilter имеет несколько переменных состояния, то IsStateVariableCircular является вектором - строкой. Каждый векторный элемент указывает, является ли переменная ассоциированной страны круговой. Если объект имеет только одну переменную состояния, то IsStateVariableCircular является скаляром.

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

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

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

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

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

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

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

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

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

Методы

копияСоздайте копию фильтра частиц
правильныйНастройте оценку состояния на основе измерения датчика
getStateEstimateИзвлечение лучше всего утверждает оценку и ковариацию от частиц
инициализироватьИнициализируйте состояние фильтра частиц
предсказатьПредскажите состояние робота в следующем временном шаге

Примеры

свернуть все

Скрипт Open Live Script

Создайте объект ParticleFilter и выполните прогноз и шаг исправления для оценки состояния. Фильтр частиц дает предсказанную оценку состояния на основе возвращаемого значения StateTransitionFcn. Это затем исправляет состояние на основе данного измерения и возвращаемого значения MeasurementLikelihoodFcn.

Создайте фильтр частиц со значением по умолчанию три состояния.

pf = robotics.ParticleFilter
pf = 
  ParticleFilter with properties:

           NumStateVariables: 3
                NumParticles: 1000
          StateTransitionFcn: @robotics.algs.gaussianMotion
    MeasurementLikelihoodFcn: @robotics.algs.fullStateMeasurement
     IsStateVariableCircular: [0 0 0]
            ResamplingPolicy: [1x1 robotics.ResamplingPolicy]
            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
Открыть скрипт

Используйте объект ParticleFilter отследить робота, когда он перемещается в 2D пробел. Измеренное положение имеет случайный добавленный шум. Используя predict и correct, отследите робота на основе измерения и на принятой модели движения.

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

pf = robotics.ParticleFilter;
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;

Начните цикл для предсказания и исправления предполагаемого положения на основе измерений. Передискретизация частиц происходит на основе свойства 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

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

Ссылки

[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

Документация Robotics System Toolbox
Поддержка