Используйте расширенный Фильтр Калмана, когда объектное движение следует за нелинейным уравнением состояния или когда измерения являются нелинейными функциями состояния. Простой пример - когда состояние или измерения объекта вычисляются в сферических координатах, таких как азимут, вертикальное изменение и область значений.
Расширенная формулировка Фильтра Калмана линеаризует уравнения состояния. Обновленная и ковариационная матрица состояния остается линейные функции предыдущей и ковариационной матрицы состояния. Однако матрица переходов в линейном Фильтре Калмана заменяется якобианом уравнений состояния. Якобиевская матрица не является постоянной, но может зависеть от самого состояния и время. Чтобы использовать расширенный Фильтр Калмана, необходимо задать и функцию изменения состояния и якобиан функции изменения состояния.
Примите, что существует выражение закрытой формы для предсказанного состояния как функция предыдущего состояния, средств управления, шума, и время.
Якобиан предсказанного состояния относительно предыдущего состояния
Якобиан предсказанного состояния относительно шума
Эти функции принимают более простые формы, когда шум входит линейно в уравнение обновления состояния:
В этом случае, F(w) = 1M.
В расширенном Фильтре Калмана измерение может быть нелинейной функцией состояния и шума измерения.
Якобиан измерения относительно состояния
Якобиан измерения относительно шума измерения
Эти функции принимают более простые формы, когда шум входит линейно в уравнение измерения:
В этом случае, H(v) = 1N.
Этот расширенный цикл фильтра Калмана почти идентичен линейному циклу Фильтра Калмана за исключением того, что:
Точные нелинейные функции обновления и измерения состояния используются каждый раз, когда возможный и матрица переходов заменяется якобианом состояния
Матрицы измерения заменяются соответствующими Якобианами.
Automated Driving Toolbox™ обеспечивает предопределенные функции обновления и измерения состояния, чтобы использовать в расширенном Фильтре Калмана.
| Модель движения | FunctionName | Функциональная цель |
|---|---|---|
| Постоянная скорость | constvel | Состояние постоянной скорости обновляет модель |
constveljac | Состояние постоянной скорости обновляет якобиан | |
cvmeas | Модель измерения постоянной скорости | |
cvmeasjac | Якобиан измерения постоянной скорости | |
| Постоянное ускорение | constacc | Состояние постоянного ускорения обновляет модель |
constaccjac | Состояние постоянного ускорения обновляет якобиан | |
cameas | Модель измерения постоянного ускорения | |
cameasjac | Якобиан измерения постоянного ускорения | |
| Постоянная угловая скорость вращения | constturn | Постоянное состояние угловой скорости вращения обновляет модель |
constturnjac | Постоянное состояние угловой скорости вращения обновляет якобиан | |
ctmeas | Постоянная модель измерения угловой скорости вращения | |
ctmeasjac | Постоянный-turnrate якобиан измерения |
