Фильтр Калмана

Чтобы максимально точно управлять любой динамической системой, нужны две вещи одновременно. Первая — её математическое описание: закон, по которому система меняется во времени. Вторая — мониторинг её состояния: измерения, которые говорят, где система оказалась на самом деле в данный момент времени. Проблема в том, что ни модели, ни измерениям нельзя верить абсолютно.

Модель всегда врёт — хоть немного. Она не знает всех особенностей конкретного объекта: трения, люфтов, износа, случайных толчков извне. Она предсказывает идеальную систему, а управляем мы настоящей. Измерения врут тоже. Датчик подвержен помехам, дрожит, ошибается.

Вот было бы круто соединить бульдога с носорогом — взять от модели её предсказательную силу, а от датчиков их привязку к реальности — и получить оценку лучше, чем даёт каждый источник по отдельности. Ровно это и делает фильтр Калмана: на каждом шаге он взвешивает доверие к прогнозу модели и к свежему измерению и выдаёт оптимальную по среднеквадратичному критерию оценку состояния.

Начнём с общего описания линейной динамической системы:

где $A_c$ — матрица системы, $x$ — вектор состояния, $B_c$ — матрица входа, а $u$ — управляющее воздействие (вход системы).

Запишем систему (1) в дискретном виде: разобьём время на шаги $i$ $= 0, 1, 2, \dots$ с интервалом $h$ и приблизим производную методом Эйлера — отношением приращений за шаг

выразив отсюда $x_{i+1}$, получаем рекуррентную формулу — состояние на следующем шаге через состояние на текущем

то есть непрерывные матрицы сворачиваются в дискретную матрицу перехода $A$ и дискретную матрицу управления $B$

и формула принимает окончательный вид

здесь $u_i$ — управляющее воздействие на шаге $i$.

Реальная система не следует уравнению (3). Добавим к нему случайный вектор $w_i$ — то, что модель не описывает

шум процесса $w_i$ считаем случайным с нулевым средним; меру его разброса — ковариационную матрицу $Q$, матрицу погрешности модели — определим ниже, при выводе прогноза ковариации.

Один цикл фильтра — это два такта: прогноз по модели и коррекция по измерению. Сначала прогноз. По формуле (3) сдвигаем предыдущую оценку $\hat{x}_{i-1}$ на шаг вперёд и получаем априорную оценку $\hat{x}_i^{-}$ (значок «минус» — «до учёта измерения»)

вместе с оценкой прогнозируем и её недостоверность. Введём ошибку оценивания — разницу между истинным состоянием и оценкой, — а мерой недостоверности будет её ковариационная матрица $P$ (по диагонали — дисперсии ошибок по каждой компоненте состояния)

здесь $\mathbb{E}[\,\cdot\,]$ — оператор усреднения (математическое ожидание): он берёт случайную величину и возвращает её среднее по всем возможным реализациям. Одиночная ошибка $e_i$ случайна и непредсказуема, но усреднение матрицы $e_i e_i^{\mathsf{T}}$ по ансамблю сглаживает случайность и оставляет устойчивую характеристику разброса — ковариацию.

Посмотрим, во что превращается эта ошибка за шаг прогноза. Истинное состояние идёт по уравнению (4) со сдвигом индекса, $x_i$ $= A x_{i-1}$ $+ B u_{i-1}$ $+ w_{i-1}$, а наша оценка — по формуле (5); вычтем вторую из первой. Одинаковые члены $B u_{i-1}$ сокращаются, и остаётся

то есть априорная ошибка — это ошибка на предыдущем шаге $e_{i-1}$, пропущенная через динамику $A$, плюс свежая погрешность модели $w_{i-1}$. Возьмём ковариацию обеих частей. Прошлая ошибка и новый шум независимы, поэтому перекрёстные члены $\mathbb{E}[e_{i-1} w_{i-1}^{\mathsf{T}}]$ обнуляются, а два оставшихся — это $P_{i-1}$ и $Q$ по их определениям

подставив $\mathbb{E}[e_{i-1} e_{i-1}^{\mathsf{T}}]$ $= P_{i-1}$ и $\mathbb{E}[w_{i-1} w_{i-1}^{\mathsf{T}}]$ $= Q$, получаем формулу прогноза ковариации: старая неопределённость проносится через $A$, к ней добавляется погрешность модели $Q$

по диагонали $Q$ стоят дисперсии погрешности по каждой компоненте состояния, вне диагонали — их взаимные ковариации. Большая $Q$ означает «модели не верю, она грубая»; маленькая — «модель точна». Аналитически вывести $Q$ из первых принципов почти никогда не удаётся, поэтому её выбирают эмпирически — подбором по поведению фильтра на реальных или смоделированных данных. Это главная настроечная ручка, и обычно $Q$ берут диагональной, лишь бы не гадать про ковариации.

Пара слов про саму $P$: маленькая означает «оценке доверяю», большая — «оценка ненадёжна». Фильтр ведёт её сам — на прогнозе она растёт (формула (6) добавляет $Q$), а на шаге коррекции уменьшится, когда измерение принесёт информацию. Стартовое $P_0$ берут заведомо большим: начальное состояние известно плохо, поэтому первым шагам фильтр не доверяет — быстро подтягивается к измерениям и «прогревается».

Обычно мы не наблюдаем весь вектор состояния $x$ целиком. Наблюдаются лишь некоторые величины — вектор измерений $y$, и его размерность меньше размерности состояния. Связь измерения с состоянием задаёт матрица наблюдения $H$

роль $H$ — согласовать размерности: она отображает $n$-мерное состояние $x$ в $m$-мерное измерение $y$ (число строк $m$ $\le n$), выбирая и линейно комбинируя те компоненты состояния, которые реально измеряются. Это и есть главный смысл наблюдателя: восстанавливать то, что не измеряется.

Датчик не выдаёт $H x_i$ точно — измерение искажено помехой $v_i$

про шум $v_i$ удобно предполагать нормальное (гауссово) распределение с нулевым средним — тогда фильтр Калмана оказывается ещё и оптимальной байесовской оценкой. Но это не обязательное условие: при любом другом распределении с конечной дисперсией фильтр остаётся наилучшей линейной оценкой по среднеквадратичному критерию. Разброс помех задаёт ковариационная матрица шума измерений

строится она из дисперсий датчиков. Если помехи разных датчиков независимы, $R$ диагональна: на диагонали — квадраты стандартных отклонений (паспортная точность прибора), вне диагонали — нули

чем точнее датчик, тем меньше его $\sigma^2$ и тем сильнее фильтр будет доверять этому каналу измерения.

Теперь всё готово, чтобы сформулировать сам фильтр. К шагу $i$ у нас два независимых источника сведений об искомом состоянии $x$. Первый — прогноз модели $\hat{x}_i^{-}$ с ковариацией ошибки $P_i^{-}$: истинное $x$ лежит где-то вокруг $\hat{x}_i^{-}$ с разбросом $P_i^{-}$. Второй — измерение $y_i$ $= H x$ $+ v$ с ковариацией шума $R$: оно указывает на $x$ с точностью $R$. Оба неточны — ищем состояние, наиболее правдоподобное сразу для обоих.

Будем считать ошибки обоих источников гауссовыми (для шума измерений мы это уже предположили). Тогда каждый источник задаёт плотность вероятности вида $\exp\!\left(-\tfrac12\, e^{\mathsf{T}} \Sigma^{-1} e\right)$, где $e$ — его ошибка, а $\Sigma$ — ковариация. Обратная ковариация $\Sigma^{-1}$ играет роль веса: чем надёжнее источник (меньше разброс), тем сильнее он тянет оценку к себе. Правдоподобие состояния $x$ — произведение двух таких плотностей

наиболее правдоподобное $x$ максимизирует это произведение. Логарифм превращает произведение в сумму показателей, а максимум $p(x)$ отвечает минимуму $-2\ln p(x)$; отбросив константы, получаем ровно сумму двух квадратичных форм — это и есть наш функционал

оптимальная оценка минимизирует $J$. Возьмём производную по $x$ и приравняем к нулю

раскрыв скобки и собрав $x$ в левой части, получаем нормальное уравнение

матрица в квадратных скобках — это в точности обратная апостериорная ковариация $(P_i)^{-1}$ (информационная форма: информация прогноза плюс информация измерения). Отсюда сразу видно и правило обновления неопределённости

Обращать матрицы размера состояния дорого. Применив лемму об обращении матриц, перепишем решение (13)–(14) в вычислительно удобной форме через коэффициент усиления Калмана $K_i$

тогда коррекция оценки — это прогноз плюс поправка, пропорциональная невязке $(y_i - H \hat{x}_i^{-})$ — расхождению между тем, что предсказала модель, и тем, что показал датчик

а ковариация ошибки после коррекции уменьшается

смысл $K$ — регулятор доверия. Если измерение точное ($R$ мала), $K$ велик и оценка тянется к датчику; если прогноз надёжен ($P^{-}$ мала), $K$ мал и фильтр держится модели. Тот самый компромисс между бульдогом и носорогом, только вычисленный оптимально.