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

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

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

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

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

$\begin{array}{l} m \ddot{x} = f \\ \ddot{x} = \frac{f}{m} = a \end{array}$

Если вы задаете состояние как

$\begin{array}{l} x_{1} = x \\ x_{2} = \dot{x}, \end{array}$

можно написать закон Ньютона в форме пространства состояний.

$\frac{d}{d t} [\begin{matrix} x_{1} \\ x_{2} \end{matrix}] = [\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}] [\begin{matrix} x_{1} \\ x_{2} \end{matrix}] + [\begin{matrix} 0 \\ 1 \end{matrix}] a$

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

$\frac{d}{d t} [\begin{matrix} x_{1} \\ x_{2} \end{matrix}] = [\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}] [\begin{matrix} x_{1} \\ x_{2} \end{matrix}] + [\begin{matrix} 0 \\ 1 \end{matrix}] a + [\begin{matrix} 0 \\ 1 \end{matrix}] v_{k}$

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

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

$\frac{d}{d t} [\begin{matrix} x_{1} \\ x_{2} \\ y_{1} \\ y_{2} \end{matrix}] = [\begin{matrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{matrix}] [\begin{matrix} x_{1} \\ x_{2} \\ y_{1} \\ y_{2} \end{matrix}] + [\begin{matrix} 0 \\ a_{x} \\ 0 \\ a_{y} \end{matrix}] + [\begin{matrix} 0 \\ v_{x} \\ 0 \\ v_{y} \end{matrix}]$

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

Когда вы переходите к дискретному времени, вы интегрируете уравнения движения по длине временного интервала. В дискретной форме для интервала расчета T представление состояния становится

$[\begin{matrix} x_{1, k + 1} \\ x_{2, k + 1} \end{matrix}] = [\begin{matrix} 1 & T \\ 0 & 1 \end{matrix}] [\begin{matrix} x_{1, k} \\ x_{2, k} \end{matrix}] + [\begin{matrix} 0 \\ T \end{matrix}] a + [\begin{matrix} 0 \\ 1 \end{matrix}] \tilde{v}$

Величина _xk+1 является состоянием в дискретном временном k+1, и _xk является состоянием в более раннем дискретном времени, k. Если вы включаете шум, уравнение становится сложнее, потому что интегрирование шума не является простым.

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

$x_{k + 1} = F_{k} x_{k} + G_{k} u_{k} + v_{k}$

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

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

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

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

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

$z_{k} = H_{k} x_{k} + w_{k}$

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

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

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

$x_{k + 1} = F_{k} x_{k} + G_{k} u_{k} .$

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

$z_{k} = H_{k} x_{k}$

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

Цикл фильтра

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

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

$x_{k + 1 | k} = F_{k} x_{k | k} + G_{k} u_{k} .$
Распространите ковариация матрицу.

$P_{k + 1 | k} = F_{k} P_{k | k} F_{k}^{T} + Q_{k} .$
Обозначение k+1|k индекса указывает, что величина является оптимальной оценкой на k+1 шаге, распространенном из шага k. Эта оценка часто называется априорной оценкой.
Затем спрогнозируйте измерение в обновленное время.

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

$S_{k + 1} = H_{k + 1} P_{k + 1 | k} H_{k + 1}^{T} + R_{k + 1}$
Тогда усиление Калмана

$K_{k + 1} = P_{k + 1 | k} H_{k + 1}^{T} S_{k + 1}^{- 1}$
и получают из использования условия оптимальности.
Исправьте предсказанную оценку с помощью измерения. Предположим, что оценка является линейной комбинацией предсказанного состояния и измерения. В оценке после коррекции используется индекс обозначения, k+1|k+1. вычисляется из

$x_{k + 1 | k + 1} = x_{k + 1 | k} + K_{k + 1} (z_{k + 1} - z_{k + 1 | k})$
где _Kk+1 - коэффициент усиления Калмана. Исправленное состояние часто называют апостериорной оценкой состояния, потому что оно выводится после включения измерения.
Исправьте ковариационную матрицу состояний

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

Этот рисунок результирующих операций цикла Калмана.

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

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

$[\begin{matrix} x_{k + 1} \\ v_{x, k + 1} \\ y_{k + 1} \\ v_{y, k + 1} \\ z_{k + 1} \\ v_{z, k + 1} \end{matrix}] = [\begin{matrix} 1 & T & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & T & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & T \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{k} \\ v_{x, k} \\ y_{k} \\ v_{y, k} \\ z_{k} \\ v_{z, k} \end{matrix}]$

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

$[\begin{matrix} m_{x, k} \\ m_{y, k} \\ m_{z, k} \end{matrix}] = [\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \end{matrix}] [\begin{matrix} x_{k} \\ v_{x, k} \\ y_{k} \\ v_{y, k} \\ z_{k} \\ v_{z, k} \end{matrix}]$

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

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

$[\begin{matrix} x_{k + 1} \\ v_{x, k + 1} \\ a_{x, k + 1} \\ y_{k + 1} \\ v_{y, k + 1} \\ a_{y, k + 1} \\ z_{k + 1} \\ v_{z, k + 1} \\ a_{z, k + 1} \end{matrix}] = [\begin{matrix} 1 & T & \frac{1}{2} T^{2} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & T & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & T & \frac{1}{2} T^{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & T & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & T & \frac{1}{2} T^{2} \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & T \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{k} \\ v_{x, k} \\ a_{x, k} \\ y_{k} \\ v_{y, k} \\ a_{y, k} \\ z_{k} \\ v_{z, k} \\ a_{z, k} \end{matrix}]$

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

$[\begin{matrix} m_{x, k} \\ m_{y, k} \\ m_{z, k} \end{matrix}] = [\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \end{matrix}] [\begin{matrix} x_{k} \\ v_{x, k} \\ a_{x, k} \\ y_{k} \\ v_{y, k} \\ a_{y, k} \\ z_{k} \\ v_{z, k} \\ a_{y, k} \end{matrix}]$

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