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

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

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

Для большинства типов объектов, прослеженных в Automated Driving Toolbox™, вектор состояния состоит из одного - 2D или 3D положения и скорости.

Начните с уравнений Ньютона для объекта, перемещающегося в 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[nknkT]}.

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

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

$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}]$

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