Расширенные фильтры Калмана

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

Модель обновления состояния

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

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

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

Якобиан предсказанного состояния относительно предыдущего состояния

F(x)=fx.

Якобиан предсказанного состояния относительно шума

F(w)=fwi.

Эти функции принимают более простые формы, когда шум входит линейно в уравнение обновления состояния:

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

В этом случае, F(w) = 1M.

Модель измерения

В расширенном Фильтре Калмана измерение может быть нелинейной функцией состояния и шума измерения.

zk=h(xk,vk,t)

Якобиан измерения относительно состояния

H(x)=hx.

Якобиан измерения относительно шума измерения

H(v)=hv.

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

zk=h(xk,t)+vk

В этом случае, H(v) = 1N.

Расширенный цикл фильтра Калмана

Это - расширенный цикл фильтра Калмана, почти идентично линейному циклу Фильтра Калмана за исключением того, что:

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

  • Матрицы измерения заменяются соответствующими Якобианами.

Предопределенные расширенные функции фильтра Калмана

Automated Driving Toolbox™ обеспечивает предопределенные функции обновления и измерения состояния, чтобы использовать в расширенном Фильтре Калмана.

Модель движенияFunctionName Функциональная цель
Постоянная скоростьconstvelСостояние постоянной скорости обновляет модель
constveljacСостояние постоянной скорости обновляет якобиан
cvmeasМодель измерения постоянной скорости
cvmeasjacЯкобиан измерения постоянной скорости
Постоянное ускорениеconstaccСостояние постоянного ускорения обновляет модель
constaccjacСостояние постоянного ускорения обновляет якобиан
cameasМодель измерения постоянного ускорения
cameasjacЯкобиан измерения постоянного ускорения
Постоянная угловая скорость вращенияconstturnПостоянное состояние угловой скорости вращения обновляет модель
constturnjacПостоянное состояние угловой скорости вращения обновляет якобиан
ctmeasПостоянная модель измерения угловой скорости вращения
ctmeasjacПостоянный-turnrate якобиан измерения

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