PatientZero 21 дек 2021 в 11:00

Объяснение фильтра Калмана в картинках

9 мин

49K

Перевод

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

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

Что это такое?

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

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

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

Я начну с расплывчатого примера, решить который можно при помощи фильтра Калмана, но если вы хотите перейти сразу к красивым картинкам и математике, то можете пропустить этот раздел.

Что можно сделать при помощи фильтра Калмана?

Давайте рассмотрим пример: вы создали маленького робота, который может блуждать по лесу; чтобы выполнять перемещение, ему точно нужно знать, где он находится.

Наш маленький робот

Допустим, наш робот имеет состояние $\vec{x_k}$ , то есть просто позицию и вектор скорости:

$\vec{x_k} = (\vec{p}, \vec{v})$

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

Наш робот также имеет датчик GPS, обладающий точностью порядка 10 метров, и это хорошо, но ему нужно знать своё местоположение более точно в этом диаметре 10 метров. В этом лесу много оврагов и обрывов, поэтому если робот ошибётся на несколько метров, он может упасть с обрыва. Поэтому самого по себе GPS недостаточно.

О нет!

Также мы можем узнать кое-что о том, как двигается робот: он знает команды, передаваемые двигателям колёс, и знает, что если он направляется в одну сторону и ему ничего не мешает, то в следующее мгновение он с большой вероятностью продвинется в том же направлении. Но, разумеется, он не знает ничего о своём движении: его может сдуть ветром, его колёса могут немного завязнуть или скатиться по кочкам; поэтому количество оборотов колёс может и неточно отражать перемещение робота, и это предсказание будет неидеальным.

Датчик GPS сообщает нам информацию о состоянии, но только косвенно, с долей неопределённости или неточности. Наш прогноз сообщает нам нечто о том, как движется робот, но только косвенно, с долей неопределённости или неточности.

Но если воспользоваться всей доступной нам информацией, сможем ли мы получить более точный ответ, чем каждая из приблизительных оценок по отдельности? Разумеется, ответ положительный, и вот для чего нужен фильтр Калмана.

Как вашу задачу видит фильтр Калмана

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

$\vec{x} = \begin{bmatrix} p\\ v \end{bmatrix}$

Мы не знаем истинных позиции и скорости; истинными могут быть целое множество возможных комбинаций позиции и скорости, но некоторые из них вероятнее других:

Фильтр Калмана предполагает, что обе переменные (в нашем случае это позиция и скорость) случайны и имеют гауссово распределение. Каждая переменная имеет среднее значение $\mu$ , которое является центром случайного распределения (и наиболее вероятным состоянием), а также дисперсию $\sigma^2$ , являющуюся мерой неопределённости:

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

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

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

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

Эта корреляция задаётся ковариационной матрицей. Если вкратце, то каждый элемент матрицы $\Sigma_{ij}$ является степенью корреляции между i-той переменной состояния и j-той переменной состояния. (Как вы могли догадаться, ковариационная матрица симметрична, то есть от перемены местами i и j ничего не изменится). Ковариационные матрицы часто обозначаются как $\mathbf{\Sigma}$ , поэтому мы обозначим их элементы как $\Sigma_{ij}$ .

Описываем задачу с помощью матриц

Мы моделируем наше знание о состоянии как гауссово распределение, поэтому нам нужно два элемента информации во время $inline$ : мы назовём нашу наилучшее возможное значение $\mathbf{\hat{x}_k}$ (среднее, также обозначаемое как $\mu$ ), а ковариационную матрицу назовём $\mathbf{P_k}$ .

(Разумеется, мы используем здесь только позицию и скорость. но полезно помнить, что состояние может содержать любое количество переменных и описывать всё, что хотите).

Далее нам нужен какой-то способ, позволяющий взглянуть на текущее состояние (во время k-1) и спрогнозировать новое состояние во время k. Помните, мы не знаем, какое состояние является «реальным», но нашей функции прогнозирования это неважно. Она просто прорабатывает все состояния и возвращает нам новое распределение:

Мы можем представить этот шаг прогнозирования с помощью матрицы $\mathbf{F_k}$ :

Она получает каждую точку в нашем исходном возможном значении и перемещает её в новое предсказанное место, то есть в то, куда система переместилась бы, если бы исходное возможное значение было истинным.

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

Иными словами:

Теперь у нас есть матрица проецирования, дающая следующее состояние, но мы по-прежнему не знаем, как обновить ковариационную матрицу.

И здесь нам нужна другая формула. Если мы перемножим каждую точку в распределении на матрицу $\color{firebrick}{\mathbf{A}}$ , то что произойдёт с ковариационной матрицей $\Sigma$ ?

Это просто. Я просто покажу вам тождественное равенство:

$Cov(x) = \Sigma\\ Cov(\color{firebrick}{\mathbf{A}}x) = \color{firebrick}{\mathbf{A}} \Sigma \color{firebrick}{\mathbf{A}}^T$

То есть объединив это с предыдущим, мы получим:

Внешнее воздействие

Однако мы описали не всё. Могут присутствовать изменения, не связанные с самим состоянием — на систему может влиять внешний мир.

Например, если состояние моделирует движение поезда, машинист может изменить скорость, что приведёт к ускорению поезда. Аналогично, в нашем примере с роботом, ПО навигации может отдать команду вращать или остановить колёса. Если мы знаем эту дополнительную информацию о том, что происходит в мире, можно засунуть её в вектор под названием $\color{darkorange}{\vec{\mathbf{u}_k}}$ , проделать с ним некие операции и прибавить его к нашему прогнозу как коррекцию.

Допустим, мы знаем ожидаемое ускорение $\color{darkorange}{a}$ , вызванное изменением скорости поезда или командами управления. Из простой кинематики мы получаем:

В матричном виде:

$\mathbf{B}_k$ называется матрицей управления, а $\color{darkorange}{\vec{\mathbf{u}_k}}$ — вектором управления. (Для любой простой системе без внешнего воздействия их можно опустить).

Давайте добавим ещё подробностей. Что произойдёт, если наш прогноз не является на 100% точной моделью происходящего на самом деле?

Внешняя неопределённость

Всё замечательно, если состояние развивается в соответствии с собственными свойствами. Всё по-прежнему замечательно, если состояние развивается в соответствии с внешними силами, если мы знаем, каковы эти внешние силы.

Но как насчёт сил, которых мы не знаем? Если мы следим, например, за квадрокоптером, его может унести ветром. Если мы следим за колёсным роботом, то колёса могут проскальзывать или его могут замедлять кочки на земле. Мы не можем отслеживать подобные аспекты, и если что-то из этого произойдёт, наш прогноз может оказаться ошибочным, потому что мы не учитываем эти дополнительные силы.

Мы можем смоделировать вызванную «миром» неопределённость (то есть то, что мы не отслеживаем), добавляя новую неопределённость после каждого шага прогнозирования:

Каждое состояние в нашем исходном возможном значении может переместиться в диапазон состояний. Так как нам очень нравятся гауссовы распределения, мы можем сказать, что каждая точка в $\color{royalblue}{\mathbf{\hat{x}}_{k-1}}$ перемещается куда-то внутри гауссова распределения с ковариацией $\color{mediumaquamarine}{\mathbf{Q}_k}$ . Иными словами можно сказать, что мы считаем неотслеживаемые воздействия как шум с ковариацией $\color{mediumaquamarine}{\mathbf{Q}_k}$ .

Так мы создаём новое гауссово распределение с другой ковариацией (но с тем же средним значением):

Расширенную ковариацию мы получим, просто прибавив $\color{mediumaquamarine}{\mathbf{Q}_k}$ , что даст нам полное выражение шага прогнозирования:

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

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

Отлично, всё довольно просто. У нас есть нечёткое возможное значение того, где может находиться наша система, заданное $\color{deeppink}{\mathbf{\hat{x}}_k}$ и $\color{deeppink}{\mathbf{P}_k}$ . Что произойдёт, когда мы получим данные от датчиков?

Уточняем возможное значение на основе измерений

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

Стоит заметить, что единицы измерения и масштаб показаний может отличаться от единиц измерения и масштаба состояния, которое мы отслеживаем. Возможно, вы уже догадались, к чему всё идёт: мы смоделируем датчики при помощи матрицы $\mathbf{H}_k$ .

Мы можем вычислить распределение показаний датчиков, которые ожидаем увидеть, привычным образом:

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

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

Мы обозначим ковариацию этой неопределённости (например, шум датчиков) как $\color{mediumaquamarine}{\mathbf{R}_k}$ . Распределение имеет среднее значение, равное наблюдаемому показанию, которое мы назовём $\color{yellowgreen}{\vec{\mathbf{z}_k}}$ .

То есть теперь у нас есть два гауссовых распределения: одно окружает среднее значение преобразованного прогноза, а второе окружает действительные показания датчика.

Мы должны попытаться пересмотреть наше предположение о показателях, которые мы увидим, на основании прогнозируемого состояния (розовое) с другим предположением, основанным на показаниях датчиков (зелёным), которые мы наблюдаем.

Каким же будет наиболее вероятное новое состояние? Для каждого возможного показания $inline$ есть две связанные с ним вероятности: (1) вероятность того, что наше показание датчика $\color{yellowgreen}{\vec{\mathbf{z}_k}}$ является (ошибочным) измерением $inline$ и (2) вероятность того, что наше предыдущее возможное значение считает $inline$ показанием, которое мы должны увидеть.

Если у нас есть две вероятности и мы хотим узнать вероятность того, что обе они истинны, нужно просто перемножить их. То есть мы берём два гауссова распределения и перемножаем их:

То, что у нас осталось — это наложение — область, где оба распределения ярки/вероятны. И это гораздо точнее, чем оба наших предыдущих возможных значения. Среднее значение распределения является конфигурацией, для которой оба возможных значения наиболее вероятны, и, следовательно, наилучшим предположением об истинной конфигурации с учётом всей имеющейся у нас информации.

Хм… Похоже на ещё одно гауссово распределение.

Оказывается, если перемножить два гауссовых распределения с собственными средними значениями и ковариационными матрицами, мы получим новое гауссово распределение с собственным средним значением и ковариационной матрицей! Возможно, вы уже видите, к чему всё идёт: должна существовать формула для получения этих новых параметров из старых!

Объединяем гауссианы

Давайте найдём эту формулу. Проще всего сначала взглянуть на это в одном измерении. Одномерная кривая Гаусса с дисперсией $\sigma^2$ и средним значением $\mu$ задаётся следующим образом:

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

Можно подставить в это уравнение предыдущее и выполнить алгебраические операции (аккуратно нормализуя, чтобы суммарная вероятность равнялась 1), чтобы получить:

Мы можем упростить уравнение, вынеся за скобки небольшую часть и обозначив её $\color{purple}{\mathbf{k}}$ :

Обратите внимание, что мы можем взять предыдущее возможное значение и прибавить нечто, чтобы создать новое возможное значение. И посмотрите, насколько проста эта формула!

Но как насчёт матричного вида? Давайте просто перепишем два предыдущих уравнения в матричном виде. Если $\Sigma$ — это ковариационная матрица гауссового распределения, а $\vec{\mu}$ — среднее вдоль каждой из осей, тогда:

$\color{purple}{\mathbf{K}}$ — это матрица, называемая коэффициентом усиления Калмана; скоро мы её используем.

Всё просто! И мы уже почти закончили!

Объединяем всё вместе

У нас есть два распределения: спрогнозированное измерение с $(\color{fuchsia}{\mu_0}, \color{deeppink}{\Sigma_0}) = (\color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T})$ и наблюдаемое измерение с $(\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})$ . Мы можем просто подставить их в последнее уравнение, чтобы найти их наложение:

И из этого коэффициент усиления Калмана равен:

Мы можем убрать $\mathbf{H}_k$ из начала каждого члена в двух предыдущих уравнениях (обратите внимание, что один скрывается в $\color{purple}{\mathbf{K}}$ ), а $\mathbf{H}_k^T$ убрать из конца всех членов уравнения $\color{royalblue}{\mathbf{P}_k’}$ .

…и мы получаем полные уравнения для шага обновления.

Вот и всё! $\color{royalblue}{\mathbf{\hat{x}}_k’}$ — это наше новое наилучшее возможное значение, и мы можем дальше передавать его (вместе с $\color{royalblue}{\mathbf{P}_k’}$ на ещё один этап прогнозирования или обновления любое количество раз.

Схема потоков информации фильтра Калмана

Подведём итог

Из всей представленной выше математики вам достаточно реализовать уравнения $\color{deeppink}{\mathbf{\hat{x}}_k}$ , $\color{royalblue}{\mathbf{\hat{x}}_k’}$ , $\color{royalblue}{\mathbf{P}_k’}$ и $\color{purple}{\mathbf{K}’}$ . (Или если вы их забыли, можно вывести всё заново из уравнений $\color{deeppink}{\mathbf{\hat{x}}_k}$ , $\color{deeppink}{\mathbf{P}_k}$ , $\color{royalblue}{\vec{\mu}’}$ и $\color{mediumblue}{\Sigma’}$ .)

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

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

Теги:

Хабы:

Если эта публикация вас вдохновила и вы хотите поддержать автора — не стесняйтесь нажать на кнопку