![image](https://habrastorage.org/getpro/habr/post_images/954/bea/314/954bea3147c72502022920b95819621b.jpg)
Я обязан рассказать вам о фильтре Калмана, потому что он выполняет просто потрясающую задачу.
Как ни удивительно, о нём, похоже, знают немногие разработчики ПО и учёные, и это печалит меня, потому что это очень обобщённый и мощный инструмент для объединения информации в условиях присутствия неопределённости. Иногда его способность извлечения точной информации кажется почти магической, а если вы думаете, что я слишком много болтаю, то взгляните на это видео, в котором я показываю, как фильтр Калмана определяет ориентацию свободно плавающего тела, посмотрев на его вектор скорости. Потрясающе!
Что это такое?
Фильтр Калмана можно использовать любой сфере, где есть неопределённая информация о какой-то динамической системе, и вы можете сделать обоснованное предположение о том, что система будет делать дальше. Даже если в дело вмешивается хаотичная реальность и влияет на предположенное нами чёткое движение, фильтр Калмана часто достаточно неплохо справляется с предсказанием того, что на самом деле произойдёт. И он пользуется корреляциями между безумными явлениями, использовать которые вы могли даже не додуматься!
Фильтры Калмана идеальны для непрерывно меняющихся систем. Они не занимают слишком много памяти (потому что им не нужно хранить историю, кроме как предыдущего состояния) и очень быстры, благодаря чему они хорошо подходят для задач реального времени и встраиваемых систем.
В большинстве статей, которые вы найдёте в Google, математика реализации фильтра Калмана выглядит довольно пугающе. И это плохо, ведь на самом деле фильтр Калмана очень легко
и просто понять, если смотреть на него под правильным углом. Поэтому он является отличной темой для статьи, и я попытаюсь раскрыть его на примере чётких и понятных изображений и цветов. От вас не требуется многого, достаточно знать основы теории вероятности и матриц.
Я начну с расплывчатого примера, решить который можно при помощи фильтра Калмана, но если вы хотите перейти сразу к красивым картинкам и математике, то можете пропустить этот раздел.
Что можно сделать при помощи фильтра Калмана?
Давайте рассмотрим пример: вы создали маленького робота, который может блуждать по лесу; чтобы выполнять перемещение, ему точно нужно знать, где он находится.
![](https://habrastorage.org/getpro/habr/post_images/940/93d/888/94093d8888908846f9fe7fba466f98a9.png)
Наш маленький робот
Допустим, наш робот имеет состояние
Обратите внимание, что состояние — это всего лишь список чисел, задающий конфигурацию нашей системы; это может быть что угодно. В нашем примере это позиция и вектор скорости, но это могут быть и данные о количестве жидкости в цистерне, температуре двигателя автомобиля, позиции пальца пользователя на тачпаде или любое количество объектов, которое вам нужно отслеживать.
Наш робот также имеет датчик GPS, обладающий точностью порядка 10 метров, и это хорошо, но ему нужно знать своё местоположение более точно в этом диаметре 10 метров. В этом лесу много оврагов и обрывов, поэтому если робот ошибётся на несколько метров, он может упасть с обрыва. Поэтому самого по себе GPS недостаточно.
![](https://habrastorage.org/getpro/habr/post_images/df6/053/d45/df6053d4546785c2db33c0a66a022c56.png)
О нет!
Также мы можем узнать кое-что о том, как двигается робот: он знает команды, передаваемые двигателям колёс, и знает, что если он направляется в одну сторону и ему ничего не мешает, то в следующее мгновение он с большой вероятностью продвинется в том же направлении. Но, разумеется, он не знает ничего о своём движении: его может сдуть ветром, его колёса могут немного завязнуть или скатиться по кочкам; поэтому количество оборотов колёс может и неточно отражать перемещение робота, и это предсказание будет неидеальным.
Датчик GPS сообщает нам информацию о состоянии, но только косвенно, с долей неопределённости или неточности. Наш прогноз сообщает нам нечто о том, как движется робот, но только косвенно, с долей неопределённости или неточности.
Но если воспользоваться всей доступной нам информацией, сможем ли мы получить более точный ответ, чем каждая из приблизительных оценок по отдельности? Разумеется, ответ положительный, и вот для чего нужен фильтр Калмана.
Как вашу задачу видит фильтр Калмана
Давайте взглянем на информацию, которую пытаемся интерпретировать. Мы продолжим с простым состоянием, содержащим только позицию и скорость.
Мы не знаем истинных позиции и скорости; истинными могут быть целое множество возможных комбинаций позиции и скорости, но некоторые из них вероятнее других:
![](https://habrastorage.org/getpro/habr/post_images/f79/31f/761/f7931f76177ec93a23b8adfbdbf3c9e1.png)
Фильтр Калмана предполагает, что обе переменные (в нашем случае это позиция и скорость) случайны и имеют гауссово распределение. Каждая переменная имеет среднее значение
![](https://habrastorage.org/getpro/habr/post_images/8eb/051/706/8eb051706a22925db4380de43274134d.png)
На этом изображении между позицией и скоростью нет корреляции, и это значит, что состояние одной переменной ничего не скажет нам о том, каким может быть состояние второй.
Показанный ниже пример демонстрирует нечто более любопытное: позиция и скорость коррелируют. Вероятность наблюдения конкретной позиции зависит от имеющейся скорости:
![](https://habrastorage.org/getpro/habr/post_images/15f/a86/573/15fa86573f39f47d64e519f013de36bc.png)
Такая ситуация может возникнуть, если, допустим, мы вычисляем новую позицию на основании старой. Если скорость была высокой, мы, вероятно, продвинулись дальше, поэтому позиция будет более удалённой. Если мы движемся медленно, то продвинемся не так далеко.
Подобную связь очень важно отслеживать, потому что она даёт нам больше информации: одно измерение сообщает нам нечто о том, каким может быть второе. В этом и заключается цель фильтра Калмана: мы хотим выжать как можно больше информации из неточных измерений!
Эта корреляция задаётся ковариационной матрицей. Если вкратце, то каждый элемент матрицы
![](https://habrastorage.org/getpro/habr/post_images/bf3/c5c/bfd/bf3c5cbfd5c791a08121e6f3220b1c4e.png)
Описываем задачу с помощью матриц
Мы моделируем наше знание о состоянии как гауссово распределение, поэтому нам нужно два элемента информации во время
![](https://habrastorage.org/webt/uv/sl/ye/uvslyeo0tcrmkiushifyqwgoglw.png)
(Разумеется, мы используем здесь только позицию и скорость. но полезно помнить, что состояние может содержать любое количество переменных и описывать всё, что хотите).
Далее нам нужен какой-то способ, позволяющий взглянуть на текущее состояние (во время k-1) и спрогнозировать новое состояние во время k. Помните, мы не знаем, какое состояние является «реальным», но нашей функции прогнозирования это неважно. Она просто прорабатывает все состояния и возвращает нам новое распределение:
![](https://habrastorage.org/getpro/habr/post_images/817/0a6/981/8170a6981244e9aa2f60d093e0d20a4e.jpg)
Мы можем представить этот шаг прогнозирования с помощью матрицы
![](https://habrastorage.org/getpro/habr/post_images/581/0ce/ee4/5810ceee40fd2cf52478551e156ec773.jpg)
Она получает каждую точку в нашем исходном возможном значении и перемещает её в новое предсказанное место, то есть в то, куда система переместилась бы, если бы исходное возможное значение было истинным.
Давайте применим этот подход. Как использовать матрицу для прогнозирования позиции и скорости в следующий момент в будущем? Мы воспользуемся очень простой кинетической формулой:
![](https://habrastorage.org/webt/pj/sw/ev/pjswevczyuapib-d4fcmksg6sug.png)
Иными словами:
![](https://habrastorage.org/webt/k5/2q/xb/k52qxb5ifjigtmjmlrk4lpkbmdg.png)
Теперь у нас есть матрица проецирования, дающая следующее состояние, но мы по-прежнему не знаем, как обновить ковариационную матрицу.
И здесь нам нужна другая формула. Если мы перемножим каждую точку в распределении на матрицу
Это просто. Я просто покажу вам тождественное равенство:
То есть объединив это с предыдущим, мы получим:
![](https://habrastorage.org/webt/1t/ts/i-/1ttsi-49yc-fw3g2-e4shw_q5c8.png)
Внешнее воздействие
Однако мы описали не всё. Могут присутствовать изменения, не связанные с самим состоянием — на систему может влиять внешний мир.
Например, если состояние моделирует движение поезда, машинист может изменить скорость, что приведёт к ускорению поезда. Аналогично, в нашем примере с роботом, ПО навигации может отдать команду вращать или остановить колёса. Если мы знаем эту дополнительную информацию о том, что происходит в мире, можно засунуть её в вектор под названием
Допустим, мы знаем ожидаемое ускорение
![](https://habrastorage.org/webt/ac/sk/gs/acskgs96m_mhph_yasrkf7ml03y.png)
В матричном виде:
![](https://habrastorage.org/webt/b6/za/vd/b6zavdjqbkgdqfd6n9by1llloqc.png)
Давайте добавим ещё подробностей. Что произойдёт, если наш прогноз не является на 100% точной моделью происходящего на самом деле?
Внешняя неопределённость
Всё замечательно, если состояние развивается в соответствии с собственными свойствами. Всё по-прежнему замечательно, если состояние развивается в соответствии с внешними силами, если мы знаем, каковы эти внешние силы.
Но как насчёт сил, которых мы не знаем? Если мы следим, например, за квадрокоптером, его может унести ветром. Если мы следим за колёсным роботом, то колёса могут проскальзывать или его могут замедлять кочки на земле. Мы не можем отслеживать подобные аспекты, и если что-то из этого произойдёт, наш прогноз может оказаться ошибочным, потому что мы не учитываем эти дополнительные силы.
Мы можем смоделировать вызванную «миром» неопределённость (то есть то, что мы не отслеживаем), добавляя новую неопределённость после каждого шага прогнозирования:
![](https://habrastorage.org/getpro/habr/post_images/3e5/d0e/a13/3e5d0ea136563cb8b15d7e4af4af1bed.jpg)
Каждое состояние в нашем исходном возможном значении может переместиться в диапазон состояний. Так как нам очень нравятся гауссовы распределения, мы можем сказать, что каждая точка в
![](https://habrastorage.org/getpro/habr/post_images/173/6db/c6b/1736dbc6bac349f1fd86de3c9e2cc76f.jpg)
Так мы создаём новое гауссово распределение с другой ковариацией (но с тем же средним значением):
![](https://habrastorage.org/getpro/habr/post_images/63b/04b/1e1/63b04b1e1330df6d2248f9272618ba8b.jpg)
Расширенную ковариацию мы получим, просто прибавив
![](https://habrastorage.org/webt/ep/ot/qu/epotqu7dk-iackwy1ydu174tw-8.png)
Иными словами, новое лучшее возможное значение является прогнозом, сделанным из предыдущего лучшего возможного значения плюс поправка на известные внешние воздействия.
А новая неопределённость прогнозируется из старой неопределённости с дополнительной неопределённостью, вызванной окружением.
Отлично, всё довольно просто. У нас есть нечёткое возможное значение того, где может находиться наша система, заданное
Уточняем возможное значение на основе измерений
Допустим, у нас есть множество датчиков, дающих нам информацию о состоянии системы. Пока нам неважно, что они измеряют; допустим, один считывает позицию, а другой скорость. Каждый датчик сообщает нам нечто косвенное о состоянии, иными словами, датчики оказывают влияние на состояние и возвращают множество показаний.
![](https://habrastorage.org/getpro/habr/post_images/b7e/5ed/bd4/b7e5edbd408b664a2ec07e95d0794227.jpg)
Стоит заметить, что единицы измерения и масштаб показаний может отличаться от единиц измерения и масштаба состояния, которое мы отслеживаем. Возможно, вы уже догадались, к чему всё идёт: мы смоделируем датчики при помощи матрицы
![](https://habrastorage.org/getpro/habr/post_images/e01/469/0db/e014690db550baa5baaf62d1d6c21e55.jpg)
Мы можем вычислить распределение показаний датчиков, которые ожидаем увидеть, привычным образом:
![](https://habrastorage.org/webt/4g/ye/be/4gyebeftwikpenpkq-4nz5tjzts.png)
Фильтры Калмана отлично справляются с шумом датчиков. Иными словами, наши датчики хотя бы немного, но ненадёжны, и каждое состояние в исходном возможном значении может привести к диапазону показаний датчиков.
![](https://habrastorage.org/getpro/habr/post_images/aaa/226/528/aaa22652895229c41402d46df09359b9.jpg)
Из каждого наблюдаемого показания мы можем предположить, что наша система находилась в определённом состоянии. Но поскольку присутствует неопределённость, некоторые состояния с большей вероятностью, чем другие могут создавать наблюдаемое показание:
![](https://habrastorage.org/getpro/habr/post_images/8f8/6b4/0e2/8f86b40e297d206f439c2c3653357c77.jpg)
Мы обозначим ковариацию этой неопределённости (например, шум датчиков) как
То есть теперь у нас есть два гауссовых распределения: одно окружает среднее значение преобразованного прогноза, а второе окружает действительные показания датчика.
![](https://habrastorage.org/getpro/habr/post_images/954/bea/314/954bea3147c72502022920b95819621b.jpg)
Мы должны попытаться пересмотреть наше предположение о показателях, которые мы увидим, на основании прогнозируемого состояния (розовое) с другим предположением, основанным на показаниях датчиков (зелёным), которые мы наблюдаем.
Каким же будет наиболее вероятное новое состояние? Для каждого возможного показания
Если у нас есть две вероятности и мы хотим узнать вероятность того, что обе они истинны, нужно просто перемножить их. То есть мы берём два гауссова распределения и перемножаем их:
![](https://habrastorage.org/getpro/habr/post_images/587/2b1/3eb/5872b13eb9fc87370a8decd02cdf0012.png)
То, что у нас осталось — это наложение — область, где оба распределения ярки/вероятны. И это гораздо точнее, чем оба наших предыдущих возможных значения. Среднее значение распределения является конфигурацией, для которой оба возможных значения наиболее вероятны, и, следовательно, наилучшим предположением об истинной конфигурации с учётом всей имеющейся у нас информации.
Хм… Похоже на ещё одно гауссово распределение.
![](https://habrastorage.org/getpro/habr/post_images/3c5/dcb/b04/3c5dcbb045192ae881e94e732d29eadc.png)
Оказывается, если перемножить два гауссовых распределения с собственными средними значениями и ковариационными матрицами, мы получим новое гауссово распределение с собственным средним значением и ковариационной матрицей! Возможно, вы уже видите, к чему всё идёт: должна существовать формула для получения этих новых параметров из старых!
Объединяем гауссианы
Давайте найдём эту формулу. Проще всего сначала взглянуть на это в одном измерении. Одномерная кривая Гаусса с дисперсией
![](https://habrastorage.org/webt/sy/8b/jx/sy8bjxq5ldkk15bnreigrfevxtg.png)
Мы хотим знать, что получается при перемножении двух гауссовых кривых. Показанная ниже синяя кривая представляет собой (ненормализованное) пересечение двух гауссовых генеральных совокупностей:
![](https://habrastorage.org/getpro/habr/post_images/855/027/a45/855027a453eb445e25ba3b8d9b11695c.png)
![](https://habrastorage.org/webt/x_/w9/nq/x_w9nqusx7e7o4z2wdnfbkcmbeq.png)
Можно подставить в это уравнение предыдущее и выполнить алгебраические операции (аккуратно нормализуя, чтобы суммарная вероятность равнялась 1), чтобы получить:
![](https://habrastorage.org/webt/fh/uh/np/fhuhnpe6dwdkzesghwckaimahsq.png)
Мы можем упростить уравнение, вынеся за скобки небольшую часть и обозначив её
![](https://habrastorage.org/webt/dt/ur/si/dtursinymttmmd6cbmc21iu8hma.png)
Обратите внимание, что мы можем взять предыдущее возможное значение и прибавить нечто, чтобы создать новое возможное значение. И посмотрите, насколько проста эта формула!
Но как насчёт матричного вида? Давайте просто перепишем два предыдущих уравнения в матричном виде. Если
![](https://habrastorage.org/webt/1n/up/e6/1nupe6ycqrvdqp1yxxi1_tg0e94.png)
Всё просто! И мы уже почти закончили!
Объединяем всё вместе
У нас есть два распределения: спрогнозированное измерение с
![](https://habrastorage.org/webt/0u/da/-p/0uda-pdpdlvzdxc2dzlbw0ihkja.png)
И из этого коэффициент усиления Калмана равен:
![](https://habrastorage.org/webt/2r/rm/kl/2rrmklcmlimkclv3rie8-pnjbyu.png)
Мы можем убрать
![](https://habrastorage.org/webt/8q/lq/8e/8qlq8ef5hyctuxgsvxbkvjthkui.png)
…и мы получаем полные уравнения для шага обновления.
Вот и всё!
![](https://habrastorage.org/getpro/habr/post_images/775/640/14d/77564014db4bb16a4cdb87334c77df92.png)
Схема потоков информации фильтра Калмана
Подведём итог
Из всей представленной выше математики вам достаточно реализовать уравнения
Это позволит вам точно смоделировать любую линейную систему. Для нелинейных систем используется расширенный фильтр Калмана, который просто линеаризует прогнозы и измерения их средних значений. (Возможно, в будущем я напишу вторую статью о расширенном фильтре Калмана).
Стоит упомянуть и отдать должное этому отличному документу, в котором применяется похожее решение с использованием наложения гауссиан. Если вам любопытно, вы можете найти там более подробный вывод формул.