trackingEKF

Расширенный Фильтр Калмана для объектного отслеживания

Описание

trackingEKF объектом является расширенный Фильтр Калмана дискретного времени, раньше отслеживал положения и скорости объектов, с которыми можно столкнуться в автоматизированном ведущем сценарии. Такие объекты включают автомобили, пешеходов, велосипеды, и стационарные структуры или препятствия.

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

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

  • Добавьте шум и в процесс и в измерения. В этом случае размеры шума процесса и шума измерения должны совпадать с размерами вектора состояния и вектора измерения, соответственно.

  • Добавьте шум в функции изменения состояния, функции модели измерения, или в обеих функциях. В этих случаях не ограничиваются соответствующие шумовые размеры.

Создание

Описание

filter = trackingEKF создает расширенный объект Фильтра Калмана для системы дискретного времени при помощи значений по умолчанию для StateTransitionFcn, MeasurementFcn, и State свойства. Шумы процесса и измерения приняты, чтобы быть дополнением.

filter = trackingEKF(transitionfcn,measurementfcn,state) задает функцию изменения состояния, transitionfcn, функция измерения, measurementfcn, и начальное состояние системы, state.

пример

filter = trackingEKF(___,Name,Value) конфигурирует свойства расширенного объекта Фильтра Калмана при помощи одного или нескольких Name,Value парные аргументы и любой из предыдущих синтаксисов. Любые незаданные свойства имеют значения по умолчанию.

Свойства

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

Состояние фильтра Калмана в виде M с действительным знаком - вектор элемента, где M является размером состояния фильтра.

Пример: [200; 0.2]

Типы данных: double

Ошибочная ковариация состояния в виде положительно-определенного M с действительным знаком-by-M матрица, где M является размером состояния фильтра. Ковариационная матрица представляет неопределенность в состоянии фильтра.

Пример: [20 0.1; 0.1 1]

Функция изменения состояния в виде указателя на функцию. Эта функция вычисляет вектор состояния на временном шаге k от вектора состояния на временном шаге k – 1. Функция может взять дополнительные входные параметры, такие как входные параметры управления или размер временного шага. Функция может также включать шумовые значения.

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

Допустимые синтаксисы (HasAdditiveProcessNoise = true)Допустимые синтаксисы (HasAdditiveProcessNoise = false)
x(k) = statetransitionfcn(x(k-1))
x(k) = statetransitionfcn(x(k-1),parameters)
  • x(k) состояние во время k.

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

x(k) = statetransitionfcn(x(k-1),w(k-1))
x(k) = statetransitionfcn(x(k-1),w(k-1),dt)
x(k) = statetransitionfcn(__,parameters)
  • x(k) состояние во время k.

  • w(k) значение для шума процесса во время k.

  • dt временной шаг trackingEKF фильтр, filter, заданный в новом вызове predict функция. dt аргумент применяется, когда вы используете фильтр в средстве отслеживания и вызываете predict функция с фильтром, чтобы предсказать состояние средства отслеживания на следующем временном шаге. Для неаддитивного случая шума процесса средство отслеживания принимает, что вы явным образом задаете временной шаг при помощи этого синтаксиса: predict(filter,dt).

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

Пример: @constacc

Типы данных: function_handle

Якобиан изменения состояния функционирует в виде указателя на функцию. Эта функция имеет те же входные параметры как функция изменения состояния.

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

Допустимые синтаксисы (HasAdditiveProcessNoise = true)Допустимые синтаксисы (HasAdditiveProcessNoise = false)
Jx(k) = statejacobianfcn(x(k))
Jx(k) = statejacobianfcn(x(k),parameters)
  • x(k) состояние во время k.

  • Jx(k) обозначает якобиан предсказанного состояния относительно предыдущего состояния. Этим якобианом является M-by-M матрица во время k. Функция Якоби может взять дополнительные входные параметры, такие как входные параметры управления или размер такта.

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

[Jx(k),Jw(k)] = statejacobianfcn(x(k),w(k))
[Jx(k),Jw(k)] = statejacobianfcn(x(k),w(k),dt)
[Jx(k),Jw(k)] = statejacobianfcn(__,parameters)
  • x(k) состояние во время k

  • w(k) демонстрационный Q - вектор элемента из шума процесса во время k. Q является размером ковариации шума процесса. Вектор шума процесса в неаддитивном случае не должен иметь тех же размерностей как вектор состояния.

  • Jx(k) обозначает якобиан предсказанного состояния относительно предыдущего состояния. Этим якобианом является M-by-M матрица во время k. Функция Якоби может взять дополнительные входные параметры, такие как входные параметры управления или размер такта.

  • Jw(k) обозначает M-by-Q якобиан предсказанного состояния относительно элементов шума процесса.

  • dt временной шаг trackingEKF фильтр, filter, заданный в новом вызове predict функция. dt аргумент применяется, когда вы используете фильтр в средстве отслеживания и вызываете predict функция с фильтром, чтобы предсказать состояние средства отслеживания на следующем временном шаге. Для неаддитивного случая шума процесса средство отслеживания принимает, что вы явным образом задаете временной шаг при помощи этого синтаксиса: predict(filter,dt).

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

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

Пример: @constaccjac

Типы данных: function_handle

Ковариация шума процесса в виде скаляра или матрицы.

  • Когда HasAdditiveProcessNoise true, задайте ковариацию шума процесса как положительный действительный скаляр или положительно-определенный M с действительным знаком-by-M матрица. M является размерностью вектора состояния. Когда задано как скаляр, матрица является кратной M-by-M единичная матрица.

  • Когда HasAdditiveProcessNoise false, задайте ковариацию шума процесса как Q-by-Q матрица. Q является размером вектора шума процесса.

    Необходимо задать ProcessNoise перед любым вызовом predict функция. В более поздних вызовах predict, можно опционально задать шум процесса как скаляр. В этом случае матрица шума процесса является кратной Q-by-Q единичная матрица.

Пример: [1.0 0.05; 0.05 2]

Опция к шуму процесса модели как дополнение в виде true или false. Когда этим свойством является true, шум процесса добавляется к вектору состояния. В противном случае шум включен в функцию изменения состояния.

Модель Measurement функционирует в виде указателя на функцию. Эта функция может быть нелинейной функцией что измерения моделей от предсказанного состояния. Входом к функции является M - вектор состояния элемента. Выходом является N - вектор измерения элемента. Функция может взять дополнительные входные параметры, такие как положение датчика и ориентация.

  • Если HasAdditiveMeasurementNoise true, задайте функцию с помощью одного из этих синтаксисов:

    z(k) = measurementfcn(x(k))
    
    z(k) = measurementfcn(x(k),parameters)
    x(k) состояние во время k и z(k) предсказанное измерение во время k. parameters аргумент обозначает все дополнительные аргументы, требуемые функцией измерения.

  • Если HasAdditiveMeasurementNoise false, задайте функцию с помощью одного из этих синтаксисов:

    z(k) = measurementfcn(x(k),v(k))
    
    z(k) = measurementfcn(x(k),v(k),parameters)
    x(k) состояние во время k и v(k) шум измерения во время k. parameters аргумент обозначает все дополнительные аргументы, требуемые функцией измерения.

Пример: @cameas

Типы данных: function_handle

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

  • Если HasAdditiveMeasurmentNoise true, задайте Функцию Якоби с помощью одного из этих синтаксисов:

    Jmx(k) = measjacobianfcn(x(k))
    
    Jmx(k) = measjacobianfcn(x(k),parameters)
    x(k) состояние во время k. Jx(k) обозначает N-by-M якобиан функции измерения относительно состояния. parameters аргумент обозначает все аргументы, требуемые функцией измерения.

  • Если HasAdditiveMeasurmentNoise false, задайте Функцию Якоби с помощью одного из этих синтаксисов:

    [Jmx(k),Jmv(k)] = measjacobianfcn(x(k),v(k))
    
    [Jmx(k),Jmv(k)] = measjacobianfcn(x(k),v(k),parameters)
    x(k) состояние во время k и v(k) R - размерный демонстрационный шумовой вектор. Jmx(k) обозначает N-by-M якобиан функции измерения относительно состояния. Jmv(k) обозначает якобиан N-by-R функция измерения относительно шума измерения. parameters аргумент обозначает все аргументы, требуемые функцией измерения.

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

Пример: @cameasjac

Типы данных: function_handle

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

  • Когда HasAdditiveMeasurementNoise true, задайте ковариацию шума измерения как скаляр или N-by-N матрица. N является размером вектора измерения. Когда задано как скаляр, матрица является кратной N-by-N единичная матрица.

  • Когда HasAdditiveMeasurementNoise false, задайте ковариацию шума измерения как R-by-R матрица. R является размером вектора шума измерения.

    Необходимо задать MeasurementNoise перед любым вызовом correct функция. После первого вызова correct, можно опционально задать шум измерения как скаляр. В этом случае матрица шума измерения является кратной R-by-R единичная матрица.

Пример: 0.2

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

Включите сглаживание состояния в виде false или true. Установка этого свойства к true требует лицензии Sensor Fusion and Tracking Toolbox™. Когда задано как true, вы можете:

  • Используйте smooth (Sensor Fusion and Tracking Toolbox) функция, обеспеченная в Sensor Fusion and Tracking Toolbox, чтобы сглаживать оценки состояния на предыдущих шагах. Внутренне, фильтр хранит результаты предыдущих шагов, чтобы позволить назад сглаживать.

  • Задайте максимальное количество сглаживания шагов с помощью MaxNumSmoothingSteps свойство фильтра отслеживания.

Максимальное количество обратного сглаживания продвигается в виде положительного целого числа.

Зависимости

Чтобы включить это свойство, установите EnableSmoothing свойство к true.

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

predictПредскажите ошибочную ковариацию оценки состояния и оценки состояния отслеживания фильтра
correctФильтр отслеживания использования правильного состояния и ковариации ошибки оценки состояния
correctjpdaФильтр отслеживания использования правильного состояния и ковариации ошибки оценки состояния и JPDA
distanceРасстояния между текущими и предсказанными измерениями отслеживания фильтра
likelihoodВероятность измерения от отслеживания фильтра
cloneСоздайте фильтр отслеживания копии
residualНевязка измерения и остаточный шум от отслеживания фильтра
initializeИнициализируйте состояние и ковариацию отслеживания фильтра

Примеры

свернуть все

Создайте двумерный trackingEKF объект и пары "имя-значение" использования, чтобы задать StateTransitionJacobianFcn и MeasurementJacobianFcn свойства. Используйте предопределенное движение постоянной скорости и модели измерения и их Якобианы.

EKF = trackingEKF(@constvel,@cvmeas,[0;0;0;0], ...
    'StateTransitionJacobianFcn',@constveljac, ...
    'MeasurementJacobianFcn',@cvmeasjac);

Запустите фильтр. Используйте predict и correct функции, чтобы распространить состояние. Можно вызвать predict и correct в любом порядке и когда много раз вы хотите. Задайте измерение в Декартовых координатах.

measurement = [1;1;0];
[xpred, Ppred] = predict(EKF);
[xcorr, Pcorr] = correct(EKF,measurement);
[xpred, Ppred] = predict(EKF);
[xpred, Ppred] = predict(EKF)
xpred = 4×1

    1.2500
    0.2500
    1.2500
    0.2500

Ppred = 4×4

   11.7500    4.7500         0         0
    4.7500    3.7500         0         0
         0         0   11.7500    4.7500
         0         0    4.7500    3.7500

Больше о

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

Алгоритмы

Расширенный Фильтр Калмана оценивает состояние процесса, которым управляет это нелинейное стохастическое уравнение:

xk+1=f(xk,uk,wk,t)

xk является состоянием на шаге k. f() является функцией изменения состояния. Случайные шумовые возмущения, wk, могут влиять на объектное движение. Фильтр также поддерживает упрощенную форму,

xk+1=f(xk,uk,t)+wk

Чтобы использовать упрощенную форму, установите HasAdditiveProcessNoise к true.

В расширенном Фильтре Калмана измерения являются также общими функциями состояния:

zk=h(xk,vk,t)

h(xk,vk,t) является функцией измерения, которая определяет измерения как функции состояния. Типичные измерения являются положением и скоростью или некоторой функцией положения и скоростью. Измерения могут также включать шум, представленный vk. Снова, фильтр предлагает более простую формулировку.

zk=h(xk,t)+vk

Чтобы использовать упрощенную форму, установите HasAdditiveMeasurmentNoise к true.

Эти уравнения представляют фактическое движение и фактические измерения объекта. Однако шумовой вклад на каждом шаге неизвестен и не может быть смоделирован детерминировано. Только статистические свойства шума известны.

Ссылки

[1] Браун, R.G. и P.Y.C. Ван. Введение в случайный анализ сигнала и прикладного Кальмана, фильтрующего. 3-й выпуск. Нью-Йорк: John Wiley & Sons, 1997.

[2] Кальман, R. E. “Новый Подход к Линейным проблемам Фильтрации и Предсказания”. Транзакции Журнала ASME Базового проектирования. Издание 82, Серия D, март 1960, стр 35–45.

[3] Блэкмен, Сэмюэль и R. Пополи. Проект и анализ современных систем слежения. Дом 1999 Artech.

[4] Блэкмен, Сэмюэль. Несколько - целевое отслеживание с радарными приложениями. Дом Artech. 1986.

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

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

Введенный в R2017a
Для просмотра документации необходимо авторизоваться на сайте