Kalman filter

В 1960 г. Рудольф Калман опубликовал свою знаменитую статью, описывающую рекурсивное решение задачи линейной фильтрации дискретных данных. С тех пор, в значительной степени благодаря достижениям в области цифровых вычислений, фильтр Калмана стал предметом обширных исследований и применений в области исследования динамических процессов.
Фильтр Калмана представляет собой набор математических уравнений, который обеспечивает эффективное вычислительное (рекурсивное) средство для оценки состояния процесса таким образом, чтобы минимизировать среднее значение квадрата ошибки. Фильтр очень мощный в нескольких аспектах: он поддерживает оценки прошлых, настоящих и даже будущих состояний, и может делать это даже тогда, когда точный характер моделируемой системы неизвестен.
Пусть \(x_i=x(t_i)\) является является некоторым дискретным процессом. Требуется на основе наблюдаемой истории \(x_0,...,x_{k-2},x_{k-1}\) найти значение \(x_k\) этого процесса в момент \(t_k\). Предсказание (прогноз), использующее предыдущие \(n\) значений обозначим через \(\hat{x}_{k,n}\).
Нетрудно видеть, что наилучшим, с точки зрения метода наименьших квадратов, значением прогноза будет среднее значение \[ \hat{x}_{k,k}=\frac{1}{k}\sum_{i=0}^{k-1}x_i. \] Построим цепочку соотношений \[ \hat{x}_{k,k}=\frac{1}{k}\left(\sum_{i=0}^{k-2}x_i+x_{k-1}\right)=\frac{1}{k}\frac{k-1}{k-1}\sum_{i=0}^{k-2}x_i+\frac{1}{k}x_{k-1}= \frac{k-1}{k}\hat{x}_{k,k-1}+\frac{1}{k}x_{k-1}=\hat{x}_{k,k-1}-\frac{1}{k}\hat{x}_{k,k-1}+\frac{1}{k}x_{k-1}= \hat{x}_{k,k-1}+\frac{1}{k}\left(x_{k-1}-\hat{x}_{k,k-1}\right). \] Таким образом, на каждом шаге прогноз строится по предыдущему \(\hat{x}_{k,k-1}\) и уточняется наблюдением \(x_{k-1}\) с корректирующим коэффициентом \(\frac{1}{k}\).
Этот факт является одним из составляющих элементов Калмановской фильтрации. Рассмотрим этот вопрос подробнее.
Вначале рассмотрим простую модельную задачку - пусть с помощью эхолота измеряем грубину водоема \(x_k\), где \(k\), соответственно, временной отсчет. Считая, что ошибка данных имеет нормальный закон (что вполне естественно), требуется построить метод, как можно лучше предсказывающий глубину водоема по предыдущим измерениям.
Ясно, что глубина изменяется по неизвестному (пока не проведены измерения) закону \(x_k\): \begin{equation}\label{1}                                          x_{k} = x_{k - 1} + h(t), \end{equation} и в каждый момент времени вместо точных значений глубины \(x_k\) мы получаем значения с ошибкой \(\varepsilon_k\) \begin{equation}\label{2}                                          z_{k} = x_{k} + \varepsilon_k. \end{equation} Задача состоит в получении оценки \(\hat{x}_k\) истинного значения \(x_k\) по предыдущим наблюдениям, если известно, что Пусть для отсчета \(k-1\) нами получено значение \(\hat{x}_{k-1}\), которое достаточно хорошо соотносится с реальной глубиной \(x_{k-1}\), тогда зная закон (\ref{1}), можно сделать предположение, что на следующем шаге эхолот даст значение, весьма похожее с \(\hat{x}_{k-1}+h_{k-1}\). С другой стороны, значение эхолота на \(k\)-м шаге, будет определяться соотношением (\ref{2}). Идея Калмана состоит в том, что "истина где-то рядом", между неточным предсказанием значения и неточным значением эхолота. Более того, пусть это значение лежит на отрезке прямой, соединяющем эти две точки \begin{equation}\label{3}                              \hat{x}_{k}=K_k z_{k}+(1-K_k)(\hat{x}_{k-1}+h_{k-1}) (K_k\in (0,1)). \end{equation} Выберем весовой коэффициент \(K_k\) так, чтобы полученное значение \(\hat{x}_{k}\) было как можно ближе к истинному \(x_k\), то есть минимизируем ошибку \(\epsilon_k=x_k-\hat{x}_{k}\): \[ \epsilon_k=x_k-\hat{x}_{k}=x_{k}-K_k (x_{k} + \varepsilon_k)-(1-K_k)(\hat{x}_{k-1}+h_{k-1})= K_k x_{k}+(1-K_k)x_k-K_k (x_{k} + \varepsilon_k)-(1-K_k)(\hat{x}_{k-1}+h_{k-1})= \] \[ =K_k x_{k}+(1-K_k)(x_{k-1}+h_{k-1})-K_k (x_{k} + \varepsilon_k)-(1-K_k)(\hat{x}_{k-1}+h_{k-1})= K_k(x_{k}-x_{k} - \varepsilon_k)+(1-K_k)(x_{k-1}+h_{k-1}-\hat{x}_{k-1}-h_{k-1})= \] \[ =(1-K_k)\epsilon_{k-1}-K_k \varepsilon_{k}. \] Для решения этой задачи используем метод наименьших квадратов, то есть, минимизируем математическое ожидание квадрата ошибки \[ E[\epsilon_k^2]=E\left[\left((1-K_k)\epsilon_{k-1}-K_k \varepsilon_{k}\right)^2\right] = E\left[(1-K_k)^2\epsilon^2_{k-1}-2(1-K_k)\epsilon_{k-1}K_k \varepsilon_{k}+K_k^2 \varepsilon^2_{k}\right]. \] Замечая, что из условий \(E[\varepsilon_k]=0\) и \(\sigma^2=E[\varepsilon_k^2]\), получаем \begin{equation}\label{4} E[\epsilon_k^2]=(1-K_k)^2E[\epsilon^2_{k-1}]+K_k^2\sigma^2. \end{equation} Найдем производную по \(K_k\) и приравняем ее нулю \[ -2(1-K_k)E[\epsilon^2_{k-1}]+2K_k\sigma^2=0, \] отсюда \[ K_k\left(E[\epsilon^2_{k-1}]+\sigma^2\right)=E[\epsilon^2_{k-1}] \] и \[ K_k=\frac{E[\epsilon^2_{k-1}]}{E[\epsilon^2_{k-1}]+\sigma^2}. \] Тогда \[     \hat{x}_{k}=\frac{E[\epsilon^2_{k-1}]}{E[\epsilon^2_{k-1}]+\sigma^2}z_{k}+ \frac{\sigma^2}{E[\epsilon^2_{k-1}]+\sigma^2}(\hat{x}_{k-1}+h_{k-1}) \] и из (\ref{4}) получаем \[ E[\epsilon_k^2]=\frac{E[\epsilon^2_{k-1}]}{E[\epsilon^2_{k-1}]+\sigma^2}\sigma^2, \] откуда легко получить оценку \[ K_k=\frac{E[\epsilon_k^2]}{\sigma^2}. \] Переписывая (\ref{3}) имеем \[ \hat{x}_{k}=\hat{x}_{k-1}+h_{k-1}+K_k(z_{k}-\hat{x}_{k-1}-h_{k-1})= \hat{x}_{k-1}+h_{k-1}+\frac{E[\epsilon_k^2]}{\sigma^2}(z_{k}-\hat{x}_{k-1}-h_{k-1}) \] то есть, на каждом шаге наблюдение \(\hat{x}_{k-1}+h_{k-1}\) уточняется наблюдением \(z_{k}\) с корректирующим коэффициентом \(\frac{E[\epsilon_k^2]}{\sigma^2}\).
Описанная задачка модельная, но в ней отражены принципы Калмановской фильтрации и параметр \(K_k\in (0,1)\), соответственно, называется коэффициентом усиления Калмана.
Калмановская фильтрация основывается на принципах:

Этап экстраполяции
Предсказание вектора состояния системы по оценке вектора состояния и примененному вектору управления с шага (k−1) на шаг k.
Этап коррекции
Вычисление отклонения полученного на шаге k наблюдения от наблюдения, ожидаемого при произведенной экстраполяции и экстраполяции вектора состояния и полученных измерений.

Перейдем непосредственно к Калмановской фильтрации.
Цель фильтра Калмана — минимизировать дисперсию оценки векторного случайного процесса \(x_k\), изменяющегося во времени: \[ x_k=A_{k-1}x_{k-1}+B_{k-1}w_k,w_k\sim N(0,Q_k),\\ z_k=H_kx_k+v_k,v_k\sim N(0,R_k),k\ge 0, \] где \(x_k\in \mathbb{R}^n\) - вектор состояния системы, \(z_k\in \mathbb{R}^m\) - вектор доступных измерений, последовательности \(\{w_0,w_1,...\}\) и \(\{v_0,v_1,...\}\) - независимый нормально распределенный белый шум с нулевым матожиданием и ковариационнми матрицами \(Q_k\ge 0\), \(R_k>0\) и \(B_k\in \mathbb{R}^{n\times s}\), при этом, последовательности \(\{w_k\}\) и \(\{v_k\}\) не зависят от начального состояния \(x_0\sim N(\bar{x}_0,\Pi_0)\). Таким образом, выполняется условие \[ E\left[ \begin{array}{c} w_k\\ v_k\\ x_0 \\ \end{array} \right][w^T_i v^T_i x^T_i 1]= \left[ \begin{array}{ccc} \left[ \begin{array}{cc} Q_k& 0 \\ 0 & R_k \\ \end{array} \right] & \delta_{k,i} & \begin{array}{cc} 0& 0 \\ 0 & 0 \\ \end{array} \\ \begin{array}{cc} 0& 0 \\ \end{array} & & \begin{array}{cc} \Pi_0& \bar{x}_0 \\ \end{array} \\ \end{array} \right], \] где \(\delta_{k,i}\) -символ Кронекера.
Стандартная реализация дискретного фильтра Калмана состоит из двух этапов: экстраполяции (предсказания оценки вектора состояния и матрицы ковариации ошибки оценивания по доступным измерениям) и фильтрации (обновления предсказанной оценки вектора состояния и матрицы ковариации ошибки оценивания в соответствии с полученным в текущий момент времени измерением).

Приведем алгоритм.

Дискретный фильтр Калмана

Ввод. Исходные данные: \(\hat{x}_{0|0}=\bar{x}_0\) и \(P_{0|0}=\Pi_0,\Pi_0>0\).
Рекуррентно обновлять значения \(k\ge 0.\)
  1. Этап фильтрации.
    \(K_k=P_kH^T_kR^{-1}_{e,k},\) \(R_{e,k}=R_k+H_kP_kH^T_k\) - нахождение коэффициента Калмана.
    \(\hat{x}_{k|k}=\hat{x}_k+K_ke_k,e_k=z_k-H_k\hat{x}_k\) - оценка.
    \(P_{k|k}=P_k-K_kH_kP_k\) - матрица ковариации ошибки.
  2. Этап экстраполяции.
    \( \hat{x}_{k+1}=A_k\hat{x}_{k|k}\)- оценка.
    \(P_{k+1}=A_kP_{k|k}A_k^T+B_kQ_kB_k^T\) - матрица ковариации ошибки.
Здесь \(\hat{x}_k\equiv \hat{x}_{k|k-1}-\) линейная среднеквадратичная экстраполяция вектора состояния \(x_k\), найденная по доступным измерениям \(\{z_0,...,z_{k-1}\}\), \[ P_k\equiv P_{k|k-1}=E\left[(x_k-\hat{x}_{k|k-1})(x_k-\hat{x}_{k|k-1})^T\right] \] ковариационная матрица ошибки оценивания на этапе экстраполяции,
\(\hat{x}_{k|k}\)- линейняя среднеквадратичная оценка вектора состояния, отфильтрованная по текущему измерению \(z_k\),
\(P_{k|k}=E\left[(x_k-\hat{x}_{k|k})(x_k-\hat{x}_{k|k})^T\right]\) - ковариационная матрица ошибки оценивания на этапе фильтрации.

Для задачи вида \[ x_k=A_{k-1}x_{k-1}+B_{k-1}u_k+w_k,w_k\sim N(0,Q_k),\\ z_k=H_kx_k+v_k,v_k\sim N(0,R_k),k\ge 0, \] где \(B_k\) - матрица управления, соответствующая вектору управляющих воздействий \(u_k\) Калмановскую фильтрацию можно проиллюстрировать следующим образом

Общая схема работы фильтра Калмана.


Полная картина работы фильтра Калмана.

Полезная литература. Книги.

  1. Brown R. Random Signals and Applied Kalman Filtering. With Matlab exercises and solutions / R.Brown, P.Hwang .— NY: John Wiley & Sons, 1997 .— 484 p.
  2. Brookner E. Tracking and Kalman Filtering Made Easy / E.Brookner .— NY: John Wiley & Sons, Inc., 1998 .— 463 p.
  3. Kordic V. Kalman Filter / V.Kordic .— Vukovar, Croatia: Intech, 2010 .— 390 p.
  4. Grewal M. Kalman Filtering: Theory and Practice Using MATLAB / M.Grewal, A.Andrews .— NY: John Wilay & Sons, 2001 .— 401 p.
  5. Балакришнан А. Теория фильтрации Калмана / А.Балакришнан .— М: Мир, 1988 .— 168 с.
  6. Браммер Л. Фильт Калмана -Бьюси. Детерминированное наблюдение и стохастическая фильтрация / Л.Браммер, Г.Зиффлинг .— М: Наука, 1982 .— 200 с.
  7. Пилипенко Н.В. Применение фильтра Калмана в нестационарной теплометрии / Н.В.Пилипенко; Учебное пособие .— СПб: ИТМО, 2017 .— 36 с.
  8. Синицын И.Н. Фильтры Калмана и Пугачева. / И.Н.Синицын; Учебное пособие .— М: Университетская книга, Логос, 2006 .— 640 с.
  9. Шахтарин Б.И. Случайные процессы в радиотехнике / Б.И.Шахтарин; Цикл лекций .— М: Радио и связь, 2000 .— 584 с.

Полезная литература. Статьи.

  1. Faragher R. Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation / R.Faragher // IEEE Signal Processing Magazine .— 2012 September .— P.128-132.
  2. KalmanFilter.NET [Електронний ресурс] .— Режим доступу: https://www.kalmanfilter.net/default.aspx
  3. Kucera V. Rudolf E. Kalman: Life and Works / V.Kucera // FAC PapersOnLine .— 2017 .— №1(50) .— P.631-636.
  4. Galanis G. A one-dimensional Kalman filter for the correction of near surface temperature forecasts / G.Galanis, M.Anadranistakis // Meteorol. Appl. .— 2002 .— №9 .— P.437-441.
  5. Simon D. Kalman Filtering / D.Simon // Embedded Systems Programming .— 2001 June .— P.72-79.
  6. The Extended Kalman Filter: An Interactive Tutorial for Non-Experts [Електронний ресурс] .— Режим доступу: http://home.wlu.edu/~levys/kalman_tutorial/
  7. Welch G. An Introduction to the Kalman Filter / G.Welch, G.Bishop // SIGGRAPH .— Los Angeles, 2001 .— P.1-47.
  8. Обидин М.В. Очистка сигнала от шумов с использованием вейвлет преобразования и фильтра Калмана / М.В.Обидин, А.Серебровский // Информационные процессы .— 2013 .— №3(13) .— C.198-205.
  9. Мусин А.Р. Сравнение качества прогнозных моделей валютного рынка с применением Калмановской фильтрации и традиционных моделей временных рядов / А.Р.Мусин // Интернет-журнал «НАУКОВЕДЕНИЕ» .— 2017 .— №3(9) .— C.1-11 .— Режим доступу: http://naukovedenie.ru/PDF/42TVN317.pdf
  10. Подивилова Е.О. Сравнение оценок минимаксного фильтра и фильтра Калмана / Е.О.Подивилова, В.Ширяев // Вестник ЮУрГУ .— 2012 .— №40 (299) .— C.182-186.
  11. Сергиенко А.Б. Алгоритмы адаптивной фильтрации: особенности реализации в MATLAB / А.Б.Сергиенко // Exponenta Pro .— 2003 .— №1(1) .— C.18-28.
  12. Семушин И.В. Устойчивые алгоритмы фильтрации – обзор и новые результаты для систем судовождения / И.В.Семушин, Ю.Цыганова, К.В.Захаров // Информационные технологии и вычислительные системы .— 2013 .— №4 .— C.90-112.
  13. Сирота А.А. Блочные алгоритмы обработки изображений на основе фильтра Калмана в задаче построения сверхразрешения / А.А.Сирота, А.Ю.Иванков // Компьютерная оптика .— 2014 .— №1(38) .— C.118-126.
  14. Тараненко Ю.К. Модель адаптивного фильтра Калмана / Ю.К.Тараненко, О.Ю.Олейник // Технология приборостроения .— 2017 .— №1 .— C.9-11.
  15. Хмарский П.А. Влияние выбора моделей входного воздействия на точность измерений вектора состояния для фильтров Калмана / П.А.Хмарский, А.С.Солонар // Доклады БГУИР .— 2012 .— №7(69) .— C.47-54.
  16. Цыганова Ю.В. О методах реализации UD-фильтра / Ю.В.Цыганова // Известия высших учебных заведений. Поволжский регион. Математика .— 2013 .— №3(27) .— C.84-104.