Линейные фильтры Калмана

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

Уравнения состояния

Для большинства типов объектов, отслеживаемых на панели инструментов, вектор состояния состоит из одно-, двух- или трехмерных положений и скоростей.

Начните с уравнений Ньютона для объекта, движущегося в направлении x при постоянном ускорении, и преобразуйте эти уравнения в форму «пространство-состояние».

$\begin{array}{l} \overset{}{mx} brh \\ \overset{}{=} \frac{fx}{} brh \end{array}$ = fm = a

Если определить состояние как

$\begin{array}{l} _{} \\ _{} \overset{x1=xx2=x˙}{}, \end{array}$

вы можете написать закон Ньютона в государственной-космической форме.

$\frac{}{ddt} \begin{matrix} [_{} \\ _{} \end{matrix} x1x2] \begin{matrix} = \\ [ \end{matrix} \begin{matrix} 0100_{]} \\ _{[} \end{matrix} \begin{matrix} x1x2 \\ ] \end{matrix} +$ [01] a

Линейная динамическая модель используется при наличии уверенности в том, что объект следует этому типу движения. Иногда модель включает в себя шум процесса для отражения неопределенности в модели движения. В этом случае уравнения Ньютона имеют дополнительный член.

$\frac{}{ddt} \begin{matrix} [_{} \\ _{} \end{matrix} x1x2] \begin{matrix} = \\ [ \end{matrix} \begin{matrix} 0100_{]} \\ _{[} \end{matrix} \begin{matrix} x1x2 \\ ] \end{matrix} + [\begin{matrix} 01 \end{matrix}]_{a}$ + [01] vk

_vk - неизвестные шумовые возмущения ускорения. Известна только статистика шума. Предполагается, что это нулевой средний гауссовский белый шум.

Этот тип уравнений можно расширить до нескольких размеров. В двух измерениях уравнение имеет вид

$\frac{}{} \begin{matrix} _{} \\ _{} \\ _{} \\ _{} \end{matrix} ddt[x1x2y1y2] = \begin{matrix} \end{matrix} [0100000000010000] \begin{matrix} _{} \\ _{} \\ _{} \\ _{} \end{matrix} [x1x2y1y2] + \begin{matrix} _{} \\ _{} \end{matrix} [0ax0ay] + \begin{matrix} _{} \\ _{[0vx0vy} \end{matrix}]$

Матрица 4 на 4 в правой части является матрицей модели перехода состояния. Для независимых x- и y- движений эта матрица является блок-диагональю.

При переходе к дискретному времени выполняется интеграция уравнений движения по длине временного интервала. В дискретной форме для интервала выборки T состояние-представление становится

$[\begin{matrix} _{x1, k} \\ +_{1x2,} \end{matrix} k \begin{matrix} + \\ 1 & ] \end{matrix} = \begin{matrix} _{[} \\ _{1T01]} \end{matrix} [\begin{matrix} \end{matrix} \begin{matrix} \end{matrix} \overset{}{}$ x1,kx2,k]+[0T]a+[01]v˜

Величина _{xk +} 1 является состоянием в дискретное время k + 1, _а xk является состоянием в более раннее дискретное время k. Если включить шум, уравнение усложняется, потому что интеграция шума не является простой.

Уравнение состояния может быть обобщено на

$_{xk +} 1_{} =_{}_{} {Fkxk}_{} +_{}$ Gkuk + vk

_Fk - матрица перехода состояния, Gk - матрица управления. Матрица управления учитывает любые известные силы, действующие на объект. Обе эти матрицы приведены. Последний член представляет шумоподобные случайные возмущения динамической модели. Предполагается, что шум является нулевым средним гауссовым белым шумом.

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

Модели измерений

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

Модель измерения предполагает, что фактическое измерение в любой момент времени связано с текущим состоянием

$_{zk} =_{}_{} {Hkxk}_{}$ + wk

_wk представляет шум измерения на текущем временном шаге. Шум измерения является также нулевым средним белым гауссовым шумом с ковариационной матрицей Q, описанной _Qk = E [_nknT].

Уравнения линейного фильтра Калмана

Без шума динамические уравнения

$_{xk +} 1_{} =_{}_{} {Fkxk}_{} +$ Gkuk.

Аналогично, измерительная модель не имеет вклада в шум измерения. В каждом случае не известны шумы процесса и измерения. Известна только статистика шума.

$_{zk} =_{}_{}$ Hkxk

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

Контур фильтра

Начните с наилучшей оценки состояния x0/0 и ковариации состояния _P0/0. Фильтр выполняет эти шаги в непрерывном цикле.

Распространение состояния на следующий шаг с помощью уравнений движения.

$_{xk + 1} |_{} k_{=}_{}_{} Fkxk 'k$ + Гкук.
Распространяйте также ковариационную матрицу.

$_{Pk + 1} |_{} k_{=}_{}^{}_{}$ FkPk 'kFkT + Qk.
Обозначение нижнего индекса k + 1 | k указывает, что величина является оптимальной оценкой на k + 1 шаге, распространяемом от шага k. Эту оценку часто называют априорной оценкой.
Затем спрогнозируйте измерение в обновленное время.

$_{zk + 1} |_{k} =_{Hk +}$ 1xk + 1 | k
Используйте разницу между фактическим и прогнозируемым измерениями для корректировки состояния в обновленное время. Коррекция требует вычисления коэффициента усиления Калмана. Для этого сначала вычислите ковариацию предсказания измерения (инновации)

$_{Sk +} 1_{=}_{Hk +}_{1Pk +}^{} 1_{|}$ kHk + 1T + Rk + 1
Тогда выигрыш Калмана

$_{Kk +} 1_{= Pk}_{+ 1}^{} |_{}^{kHk}$ + 1TSk + 1 − 1
и получают из использования условия оптимальности.
Скорректируйте прогнозируемую оценку с помощью измерения. Предположим, что оценка является линейной комбинацией предсказанного состояния и измерения. Оценка после коррекции использует обозначение нижнего индекса, k + 1 | k + 1. вычисляется из

$_{xk + 1 |} k_{+ 1} =_{xk} +_{1 |} k_{+ Kk}$ + 1 (zk + 1 − zk + 1 | k)
где _{Kk +} 1 - коэффициент усиления Калмана. Скорректированное состояние часто называют апостериорной оценкой состояния, поскольку оно выводится после включения измерения.
Коррекция ковариационной матрицы состояния

$_{}_{}_{}_{} {^{}}_{Pk+1|k+1=Pk+1|k−Kk+1Sk+1K′k+1}$
Наконец, можно вычислить измерение на основе скорректированного состояния. Это не является поправкой к измерению, а является наилучшей оценкой того, что измерение было бы основано на наилучшей оценке состояния. Сравнение этого значения с фактическим измерением позволяет получить представление о производительности фильтра.

На этом рисунке представлены операции цикла Калмана.

Модель постоянной скорости

Линейный фильтр Калмана содержит встроенную линейную модель движения с постоянной скоростью. Можно также задать матрицу перехода для линейного движения. Обновление состояния на следующем шаге времени является линейной функцией состояния в настоящее время. В этом фильтре измерения также являются линейными функциями состояния, описанного матрицей измерений. Для объекта, перемещающегося в 3-D пространстве, состояние описывается положением и скоростью в координатах x, y и z. Модель перехода состояния для движения с постоянной скоростью

$[\begin{matrix} _{} \\ _{xk+1vx,} \\ _{} \\ _{k+1yk+1vy,} \\ _{} \\ _{k+1zk+1vz, k+1} \end{matrix}] = \begin{matrix} \end{matrix} [1T0000010000001T0000010000001T000001] [\begin{matrix} _{} \\ _{xkvx,} \\ _{} \\ _{kykvy,} \\ _{} \\ _{kzkvz, k} \end{matrix}]$

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

$[\begin{matrix} _{mx,} \\ _{kmy,} \\ _{kmz, k} \end{matrix}] = \begin{matrix} \end{matrix} [100000001000000010] [\begin{matrix} _{} \\ _{xkvx,} \\ _{} \\ _{kykvy,} \\ _{} \\ _{kzkvz, k} \end{matrix}]$

Модель постоянного ускорения

Линейный фильтр Калмана содержит встроенную линейную модель движения с постоянным ускорением. Можно также задать матрицу перехода для линейного движения с постоянным ускорением. Модель перехода для линейного ускорения

$[\begin{matrix} _{} \\ _{xk+1vx,} \\ _{k+1ax,} \\ _{} \\ _{k+1yk+1vy,} \\ _{k+1ay,} \\ _{} \\ _{k+1zk+1vz,} \\ _{k+1az, k+1} \end{matrix}] = \begin{matrix} \frac{}{}^{} \\ \frac{}{}^{} \\ \frac{}{}^{} \end{matrix} [1T12T200000001T0000000010000000001T12T200000001T0000000010000000001T12T200000001T000000001] [\begin{matrix} _{} \\ _{xkvx,} \\ _{kax,} \\ _{} \\ _{kykvy,} \\ _{kay,} \\ _{} \\ _{kzkvz,} \\ _{kaz, k} \end{matrix}]$

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

$[\begin{matrix} _{mx,} \\ _{kmy,} \\ _{kmz, k} \end{matrix}] = \begin{matrix} \end{matrix} [100000000000100000000000100] [\begin{matrix} _{} \\ _{xkvx,} \\ _{kax,} \\ _{} \\ _{kykvy,} \\ _{kay,} \\ _{} \\ _{kzkvz,} \\ _{kay, k} \end{matrix}]$

См. также

Объекты

trackingKF

Документация