trackingEKF

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

Описание

A 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 функция. The 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) является выборкой < reservedrangesplaceholder1 > элемента шума процесса в момент времени k. Q - размер ковариации шума процесса. Вектор технологического шума в неаддитивном случае не должен иметь такие же размерности, как вектор состояния.

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

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

  • dt - временной шаг trackingEKF фильтр, filter, указанный в последнем вызове predict функция. The 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, технологический шум добавляется к вектору состояния. В противном случае шум включается в функцию перехода состояния.

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

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

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

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

    z(k) = measurementfcn(x(k),v(k))
    
    z(k) = measurementfcn(x(k),v(k),parameters)
    x(k) состояние в момент времени k и v(k) - измерение шум в момент времени k. The 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 якобиан функции измерения относительно состояния. The 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 измерения относительно шума измерения. The 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] Калман, Р. Е. «Новый подход к линейным задачам фильтрации и предсказания». Транзакции ASME-Journal of Basic Engineering. Том 82, серия D, март 1960, стр. 35-45.

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

[4] Блэкман, Сэмюэль. Отслеживание нескольких целей с помощью радаров. Дом Артеха. 1986.

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

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

.
Введенный в R2017a