А ваш фильтр Калмана правильно работает?

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

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

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

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

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

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

Вкратце работу фильтра Калмана можно объяснить так:

Рис. 1. Измерения, предсказываемое и оптимальное состояние.

Для описания какого-либо процесса мы используем как состояние, полученное посредством измерений (Measurement), так и состояние, полученное по уравнениям, описывающим происходящий процесс (Predicted state estimate). Комбинируя эти два независимых состояния, мы получаем более точное оптимальное состояние (Optimal state estimate).

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

В зависимости от сложности процесса и измерений можно использовать линейный, расширенный или сигма-точечный фильтр Калмана.

Проблема верификации результатов

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

Есть множество материалов, статей, диссертаций, которые рассматривают существующие или разрабатывают новые разновидности фильтра Калмана, однако, вопрос валидации результатов встречается намного реже. Зачастую авторы ограничиваются сравнением зашумленного/оценочного/реального состояния, как это показано на рис. 2 (источник), аргументируя точность фильтра близким значением оценочного и реального состояний и забывая про ковариационную матрицу ошибок. Проблема такой оценки кроется в том, что без использования ковариационной матрицы ошибок мы не можем оценить, насколько точные данные мы имеем.

Рис. 2. Пример сравнения зашумленных, оценочных и реальных данных.

Проиллюстрируем проблему на простом примере, когда состояние системы описывается одной переменной x. В таком случае, ковариационная матрица ошибок представляет из себя дисперсию _x² этой величины. Допустим, у нас x = 0 и _x = 1, тогда в соответствии с правилом 3 сигма, мы можем с уверенностью > 99.5 % сказать, что наша величина лежит в интервале [-3, 3]. Если мы увеличим среднеквадратичное отклонение _x до 10, то интервал увеличится до [-30, 30]. Если наш фильтр дает оценку x = 0 и _x = 1, в то время как действительная величина среднеквадратичного отклонения _x = 10, то наш объект может в реальности оказаться в местоположении, которое практически невозможно в соответствии с оценкой нашего фильтра. Чем это может грозить, думаю понятно без комментариев. Таким образом, дисперсия вносит существенный вклад в оценку состояния, и пренебрегать ей ни в коем случае нельзя.

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

Терминология и обозначения

Прежде чем пойти дальше, введем основные обозначения и терминологию. Подстрочный индекс k будет обозначать номер временного слоя, x_k вектор состояния системы, y_k вектор измерений, P_k ковариационная матрица ошибок, x₀, P₀ начальные значения вектора состояния и ковариационной матрицы. Динамика системы описывается дискретными уравнениями:

$\textbf{x}_k = f\left(\textbf{x}_{k-1}, \textbf{w}_k\right) \qquad \qquad \qquad (1)$

где f функция перехода для рассматриваемого процесса, w_k некоторая матрица шумов, ассоциированная с соответствующей функцией перехода. Функция f может иметь различный вид, например, в простейшем случае быть линейной как относительно x_k_-1, так и относительно w_k.

Связь между измерениями системы и вектором состояния описываются уравнением:

$\textbf{y}_k = h\left(\textbf{x}_k, \textbf{v}_k\right) \qquad \qquad \qquad (2)$

где h функция, связывающая вектор состояния с вектором измерений, а v_k некоторая матрица ошибок измерений.

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

Реальные значения вектора состояния и ковариационной матрицы мы будем обозначать x_k^* и P_k^*, в то время как значения, полученные с помощью фильтра Калмана, просто x_k и P_k.

Постановка численного эксперимента

Для валидации фильтра Калмана мы будем использовать метод Монте-Карло. Вначале мы вычислим реальные значения состояния x_k^* на каждом временном слое от 0 до n. Для практических целей это можно сделать с помощью уравнения (1), заменив w_k на нулевую матрицу и задав определенные x₀^*, P₀^*.

Затем нам нужно смоделировать m независимых численных экспериментов. Всего у нас имеется 3 источника случайности, которые мы будем варьировать за каждый проход итерации Монте-Карло, это начальное состояние (его мы знаем только с точностью, определяемой ковариационной матрицей P₀), и значения шумов, определяемые матрицами w_k и v_k. Таким образом, на каждом прогоне мы получаем различные значения начального состояния и значений шумов процесса и измерений на каждом временном слое. Номер итерации Монте-Карло мы будем обозначать буквой i. В результате моделирования мы будем иметь m произвольных наборов x_kⁱ и P_kⁱ для 0 k n и 0 i m.

Стоит заметить, что обычно при постановке задачи значения P_k^* не известны, однако их можно заменить на статистические значения, вычисленные по формуле:

$\textbf{x}_k^{avg}=1/N \sum\textbf{x}_k^i \qquad \qquad \qquad \qquad \qquad \quad (3)$ $\textbf{x}_k^{i, err}=\textbf{x}_k^i - \textbf{x}_k^{avg} \qquad \qquad \qquad \qquad \qquad \quad (4)$ $\mathbf{P}_k^{est} = 1 / N \sum \textbf{x}_k^{i, err}\left(\textbf{x}_k^{i, err}\right)^T \qquad \qquad \qquad (5)$

где надстрочный индекс T обозначает транспонирование. В дальнейшем везде под P_k^* имеется в виду P_k^est.

Верификация результатов

Стороннему наблюдателю может показаться, что мы ничего существенного не сделали, но, на самом деле, самое сложное уже позади. Осталось только понять, насколько близки значения x_kⁱ и P_kⁱ к ожидаемым значениям x_k^* и P_k^*. Для этого мы воспользуемся идеей из статьи [X. R. Li, Z. Zhao Measuring Estimator's Credibility: Noncredibility Index // In Proceedings of 2006 International Conference on Information Fusion, Florence, 2006.], которая заключается в следующем: для каждого временного слоя k и для каждой итерации Монте-Карло i мы вычисляем noncredibility index по формуле:

$\gamma_k^i = 10\left|log\left( \left[\textbf{x}_k^{i, err} \mathbf{P}_k^i \textbf{x}_k^{i, err}\right] / \left[\textbf{x}_k^{i, err}\mathbf{P}_k^*\textbf{x}_k^{i, err}\right] \right)\right| \qquad \qquad \qquad (6)$

Данная формула определяет некоторую ошибку как в случае несоответствия x_kⁱ, так и в случае несоответствия P_kⁱ, и обладает следующими полезными свойствами:

Значение _kⁱ близко к нулю, если x_kⁱ и P_kⁱ близки к действительным значениям;
Значение _kⁱ безразмерно и, следовательно, не зависит от единиц измерения x_kⁱ;
Значение _kⁱ одинаково серьезно штрафует как за оптимистичные (отношение [x_k^{i, err} P_kⁱ x_k^{i, err}] / [x_k^i,
err P_k^* x_k^{i, err}] > 1) значения, так и за пессимистичные значения P_kⁱ (когда это отношение меньше единицы).

Тогда для временного слоя k можно посчитать среднее значение noncredibility indexа:

$\gamma_k^{avg} = 1 / N \sum \gamma_k^i \qquad \qquad \qquad (7)$

Это и есть основная мера валидности результата, полученного с помощью фильтра Калмана. Таким образом, для каждого временного слоя мы можем посчитать _k^avg и по нему понять, насколько хорошо работает наш фильтр. В процитированной работе было показано, что значения _k^avg ~ 1 соответствуют достоверным результатам.

Собирая все в одну кучу

Весь процесс валидации фильтра Калмана сводится к следующим шагам:

Для поставленной задачи задаем функцию перехода f, функцию, связывающую вектор состояния с вектором измерений, h, матрицы шумов w_k и v_k и начальные значения x₀^*, P₀^*;
Проводим симуляцию без шумов для получения действительных значений x_k^*;
Запускаем m итераций Монте-Карло и получаем x_kⁱ и P_kⁱ для каждого временного слоя k и для каждой итерации i;
Вычисляем P_k^est в соответствии с формулой (5). P_k^est будет служить в качестве P_k^* ;
Для каждого временного слоя вычисляем _k^avg, которая и будет показателем достоверности результатов;
(Опционально) Строим график зависимости _k^avg от времени и смотрим, как изменялось качество работы фильтра.

Результаты

В качестве примера рассмотрим задачу отслеживания мотоцикла (рис. 3) из статьи.

Рис. 3. Кинематическая модель мотоцикла

Уравнения, описывающие кинематическую модель движения, запишутся в виде:

$\dot{x} = v\cos{\left( \psi + \beta \right)}, \qquad \qquad \qquad \qquad \qquad \qquad (8)$ $\dot{y} = v \sin{\left(\psi + \beta\right)}, \qquad \qquad \qquad \qquad \qquad \qquad (9)$ $\dot{\psi} = \frac{v}{l_r}\sin{\left(\beta\right)}, \qquad \qquad \qquad \quad \qquad \qquad \qquad (10)$ $\dot{v}=a, \qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad (11)$ $\beta = \tan^{-1}{\left(\frac{l_r}{l_r + l_f}\tan{\left(\delta_f\right)}\right)}, \qquad \qquad \qquad (12)$

где x, y координаты центра тяжести, v скорость, угол между направлением мотоцикла и осью x, угол между направлением скорости и направлением мотоцикла, a ускорение, l_r и l_f длина от центра масс до задней и передней части соответственно, _f угол между направлением переднего колеса и направлением мотоцикла.

Значения l_r и l_f являются входными параметрами конфигурации мотоцикла, _f и a значения, которые определяют динамику системы, а x, y вектор состояния.

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

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

$r = \sqrt{x^2 + y^2}+v^0, \qquad \qquad \qquad \quad (13)$ $\phi = \tan^{-1}{\left( \frac{y}{x} \right)}+v^1. \qquad \qquad \qquad (14)$

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

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

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

Рис. 4. Зависимость noncredibility index от номера итерации для чисел одинарной точности.

Как видно, значение noncredibility index растет с течением времени, что говорит о накапливаемой ошибке в результатах и, по-видимому, все большему удалению матрицы P_k от действительного значения.

Для чисел двойной точности значения _k^avg от номера итерации колебались в пределах от 0.8 до 1.6, что говорит о правдоподобности результатов (см. рис. 5). Как видно из рисунка, значения noncredibility index не растут с течением времени, а лишь колеблются в окрестности некоторого среднего значения.

Рис. 5. Зависимость noncredibility index от номера итерации для чисел двойной точности.

Вывод

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

При применении рассмотренной выше схемы валидации для ряда более сложных задач мы установили, что расширенный фильтр дает неудовлетворительные результаты, которые было бы невозможно определить на глаз простым сравнением графиков векторов состояния. Таким образом, для дальнейшего моделирования мы использовали сигма-точечный фильтр.

Лично меня подкупила простота подхода какова бы ни была задача: со сложными дифференциальными уравнениями, описывающими процесс, и большим вектором состояния, или простыми алгебраическими уравнениями и скалярным состоянием, в итоге мы получаем лишь одно число, которое наглядно показывает, насколько хорошо фильтр описывает происходящий процесс. Другой плюс рассмотренного подхода это возможность валидации не только вектора состояния x_kⁱ, но и ковариационной матрицы ошибок P_kⁱ.

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

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

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

C++ is a horrible language. It's made more horrible by the fact that a lotof substandard programmers use it, to the point where it's much mucheasier to generate total and utter crap with it.

Linus Benedict Torvalds

Собеседование шло уже второй час. Мы наконец-то закончили тягучее и вязкое обсуждение моей скромной персоны, и фокус внимания плавно переполз на предлагаемый мне проект.

Лучший тестировщик

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

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

Я столкнулся с Robot Framework около года

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

Работая в компании IT-аутсорса в качестве руководителя 3 линии поддержки, задумался, как автоматизировать подключение сотрудников по RDP, через VPN к серверам десятков клиентов.

Таблички с адресами, паролями и прочими настройками серверов, конечно, хорошо, но поиск клиента и вбивание адресов с аккаунтами занимает довольно существенное время.
Держать все подключения к VPN в Windows не самая лучшая идея, да и при переустан

Всем привет! Меня зовут Константин Измайлов, я руководитель направления Data Science в Delivery Club. Мы работаем над многочисленными интересными и сложными задачами: от формирования классических аналитических отчетов до построения рекомендательных моделей в ленте приложения.

Сегодня я расскажу пр

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

Cодержание

Введение
Основные понятия и термины
Характеристика ИО как научной дисциплины
Этапы операционного исследования
- Постановка задачи
- Построени

Можно выделить ряд алгоритмов, которые являются базовыми и лежат в основе практически каждой строчки программ, написанных на языках высокого уровня. Хорошо иметь под руками классический многотомный труд Дональда Кнута "The Art of Computer Programming", там детально разобраны многие ба

Давайте представим, что вы параноик, и параноик вдвойне, когда дело касается многопоточности. Предположим, что вы делаете backend некого функционала приложения, а приложение переодически дергает на вашем серверы какие-то методы. Все вроде хорошо, но есть одно но. Что если ваш функционал напрямую зависит от каких-либо других данных, того же банального профиля например? Встает вопрос, как гарантировать то, что сценарий отработает име

Трансформеры за последние несколько лет штурмом захватили мир NLP, а сегодня они с успехом применяются в выходящих за рамки NLP приложениях. Они обладают такими возможностями благодаря модулю внимания, который схватывает отношения между всеми словами последовательностей. Но са

Пример расчётного доказательства в Lean

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

Зарабатывать продажей лекарств, которые заведомо не работают, не только аморально, но и не особо легко. Люди всё-таки обычно не хотят покупать препараты, неэффективность которых была доказана. А вот если вы сумели выдавить заветное p < 0.05 в пользу того, что акупунктура та

В этой статье мы рассмотрим, что такое классификатор, поговорим о мультиклассовой классификации с помощью нейронных сетей. Затем, ознакомившись с контекстом перейдем к основному топику поста к Log-Sum-Exp Trick. Напишем формулы и разберемся, как этот трюк помогает избежать переполнения чисел с плавающей то

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

Призма Вельда-Бланделла

На рубеже четвертого и третьего тысячелетия до нашей эры на Земле возникли две первые цивилизации. В долине Нила после объединения верхнего и нижнего Египта образовалось

Они отличаются тем, что у гибридных (Ca+, Ca/Sb) свинцовый сплав положительных решёток легирован сурьмой, а отрицательных кальцием, тогда как у кальциевых (Ca/Ca) те и другие кальцием. В результате, выделение газов происходит при разных напряжениях заряда, и токи окончания заряда при этих напряжениях тоже разные.

Однако, современные автом

Обложка к комиксу Weird science. 50-годы

NASA разрабатывает планетоход VIPER (Volatiles Investigating Polar Exploration Rover), который будет искать и составлять карту залежей воды на Луне.

Взгляд на наше космическое будущее из 1970-х годов

В период с 1956 по 1962 годы психолог Кейптаунского университета Курт Данцигер проводил масштабный опрос. По его просьбе 436 южноафриканских школьников и студентов написали

Восставший может погрузиться вбездну, апогрузившийся вбездну может вновь восстать. (Говард Филипс Лавкрафт. Зов Ктулху)

В бездну пучин сланцевых пород скалы эпохи Велнока,что на юге графства Херефордшир (Великобритания) раз за разом п

(Примечание переводчика: не нашёл публикации (-ий) по данной теме на Хабре.)

Блоуинг Рок, Северная Каролина, 21 декабря 2018 года организация Great Internet Mersenne

^{Предтеча мультиметра гальванометр}
Многие из нас практически ежедневно использует мультиметр по работе или в ходе реализации каких-то хобби-проектов. Есть простенькие мультиметры, которые измеряют лишь силу тока и напряжение. Есть очень сложные приборы, которые, кажется, способны измерить

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

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

В последнее время появляется все больше и больше аналитических обзоров результатов выборов, которые рассматривают их с точки зрения законов статистики и направлены, как правило, на изучение необычных явлений, сигнализирующих о возможных фальсификациях (см. "Гаусс против Чурова" и т.п. публикации). Думается, только что завершившиеся выборы президента США (подсчет голосов еще продолжается, причем беспрецедентно длительное время) даду

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

	Русский
	English