Что такое фильтр калмана простым языком

Фильтр Калмана — Введение

Фильтр Калмана — это, наверное, самый популярный алгоритм фильтрации, используемый во многих областях науки и техники. Благодаря своей простоте и эффективности его можно встретить в GPS-приемниках, обработчиках показаний датчиков, при реализации систем управления и т.д.

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

Для чего он нужен?

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

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

Рассмотрим простейший пример — предположим нам необходимо контролировать уровень топлива в баке. Для этого в бак устанавливается емкостный датчик, он очень прост в обслуживании, но обладает некоторыми недостатками — например, зависимость от заправляемого топлива (диэлектрическая проницаемость топлива зависит от многих факторов, например, от температуры), большое влияние «болтанки» в баке. В итоге, информация с него представляет типичную «пилу» с приличной амплитудой. Такого рода датчики часто устанавливаются на тяжелой карьерной технике (не смущайтесь объемам бака):

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

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

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

Разберемся сначала в обозначениях: подстрочный индекс обозначает момент времени: k — текущий, (k-1) — предыдущий, знак «минус» в верхнем индексе обозначает, что это предсказанное промежуточное значение.

Описание переменных представлены на следующих изображениях:

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

Опробуем в деле

Вернемся к примеру с датчиком уровня топлива, так как состояние системы представлено одной переменной (объем топлива в баке), то матрицы вырождаются в обычные уравнения:

Определение модели процесса

Для того, чтобы применить фильтр, необходимо определить матрицы/значения переменных определяющих динамику системы и измерений F, B и H:

F — переменная описывающая динамику системы, в случае с топливом — это может быть коэффициент определяющий расход топлива на холостых оборотах за время дискретизации (время между шагами алгоритма). Однако помимо расхода топлива, существуют ещё и заправки… поэтому для простоты примем эту переменную равную 1 (то есть мы указываем, что предсказываемое значение будет равно предыдущему состоянию).

B — переменная определяющая применение управляющего воздействия. Если бы у нас были дополнительная информация об оборотах двигателя или степени нажатия на педаль акселератора, то этот параметр бы определял как изменится расход топлива за время дискретизации. Так как управляющих воздействий в нашей модели нет (нет информации о них), то принимаем B = 0.

H — матрица определяющая отношение между измерениями и состоянием системы, пока без объяснений примем эту переменную также равную 1.

Определение сглаживающих свойств

R — ошибка измерения может быть определена испытанием измерительных приборов и определением погрешности их измерения.

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

Реализуем в коде

Чтобы развеять оставшиеся непонятности реализуем упрощенный алгоритм на C# (без матриц и управляющего воздействия):

Результат фильтрации с данными параметрами представлен на рисунке (для настройки степени сглаживания — можно изменять параметры Q и R):

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

Надеюсь описание получилось не сильно утомительным и сложным, если остались вопросы и уточнения — добро пожаловать в комментарии )

Более серьезные источники:
Greg Welch, Gary Bishop, «An Introduction to the Kalman Filter», 2001
M.S.Grewal, A.P. Andrews, «Kalman Filtering — Theory and Practice Using MATLAB», Wiley, 2001

UPD2: приведенный в статье пример — чисто демонстрационный. Основное применение фильтра более сложные системы. Например, в случае определение координат автомобиля можно связать gps-координаты, угол поворота руля, обороты двигателя… и все это даст повышение точности координат.

Источник

Суть фильтра Калмана

рис.1

Таким образом, искомая нами пара лежит где-то на этой прямой. Повторюсь, выбрать из всех этих вариантов ту пару, которая была на самом деле – которая дала число 12, не владея какими-либо дополнительными подсказками, невозможно. Однако, в ситуации, для которой изобретён фильтр Калмана, такие подсказки есть. Там заранее о случайных числах кое-что известно. В частности там известна так называемая гистограмма распределения для каждой пары чисел. Она обычно бывает получена после достаточно длительных наблюдений за выпадениями этих самых случайных чисел. То есть, например, из опыта известно, что в 5% случаев обычно выпадает пара x=1, y=8 (обозначим эту пару так: (1,8)), в 2% случаев пара x=2, y=3 (2,3), в 1% случаев пара (3,1), в 0.024% случаев пара (11,1) и т.д. Повторюсь, эта гистограмма задана для всех пар чисел, в том числе и для тех, что образуют в сумме 12. Таким образом, для каждой пары, что даёт в сумме 12, мы можем сказать, что, например, пара (1, 11) выпадает в 0.8% случаев, пара (2, 10) – в 1% случаев, пара (3, 9) – в 1.5% случаев и т.д. Таким образом, мы можем по гистограмме определить, в скольких процентах случаев сумма слагаемых пары равна 12. Пусть, например, в 30% случаев сумма даёт 12. А в остальных 70% выпадают остальные пары – это (1,8), (2,3), (3,1) и т.д. – те, что в сумме дают числа отличные от 12. Причём пусть, например, пара (7,5) выпадает в 27% случаев в то время, как все остальные пары, что дают в сумме 12, выпадают в 0.024%+0.8%+1%+1.5%+…=3% случаев. Итак, по гистограмме мы выяснили, что числа дающие в сумме 12 выпадают в 30% случаев. При этом мы знаем, что если выпало 12, то чаще всего (в 27% из 30%) причиной этого является пара (7,5). То есть если уже выпало 12, то мы можем сказать, что в 90% (27% из 30% – или, что то же самое 27 раз из каждых 30-ти) причиной выпадения 12 является пара (7,5). Зная, что чаще всего причиной получения суммы равной 12 является пара (7,5) логично предположить, что, скорее всего, она выпала и сейчас. Конечно, всё-таки не факт, что на самом деле сейчас число 12 образовано именно этой парой, однако, в следующие разы, если нам попадётся 12, и мы опять предположим пару (7,5), то где-то в 90% случаев из 100% окажемся правы. А вот если мы будем предполагать пару (2, 10), то окажемся правы лишь в 1% из 30% случаев, что равно 3.33% правильных догадок по сравнению с 90% при предположении пары (7,5). Вот и всё – в этом и состоит смысл алгоритма фильтра Калмана. То есть фильтр Калмана не гарантирует, что не ошибётся в определении слагаемого по сумме, однако он гарантирует, что ошибётся минимальное количество раз (вероятность ошибки будет минимальна), так как использует статистику – гистограмму выпадения пар чисел. Так же необходимо подчеркнуть, что часто в алгоритме фильтрации Калмана используется так называемая плотность распределения вероятности (ПРВ). Однако необходимо понимать, что смысл там тот же, что и у гистограммы. Более того, гистограмма – это функция, построенная на основе ПРВ и являющаяся её приближением (см., например, здесь).
В принципе мы эту гистограмму можем изобразить в виде функции двух переменных – то есть в виде некоей поверхности над плоскостью xy. Там, где поверхность выше, там выше и вероятность выпадения соответствующей пары. На рис.2 изображена такая поверхность.

рис.2

Как видно над прямой x+y=12 (которая есть варианты пар дающих в сумме 12) расположены точки поверхности на разной высоте и наибольшая высота у варианта с координатами (7,5). И когда нам встречается сумма равная 12, в 90% случаев причиной появления этой суммы является именно пара (7,5). Т.е. именно эта пара, дающая в сумме 12, имеет наибольшую вероятность появления при условии, что сумма равна 12.
Таким образом, здесь описана идея лежащая в основе фильтра Калмана. Именно на ней и построены всевозможные его модификации – одношаговые, многошаговые рекуррентные и т.д. Для более глубокого изучения фильтра Калмана рекомендую книгу: Ван Трис Г. Теория обнаружения, оценок и модуляции.

p.s. Для того, кто интересуется объяснениями понятий математики что называется «на пальцах» можно посоветовать вот эту книгу и в частности главы из её раздела «Математика» (саму книгу или отдельные главы из неё вы можете приобрести здесь).

Комментарии

Следователь
22/12/15 16:22

Следователь
22/12/15 16:31

Аппроксимация частотных характеристик цифровых фильтров с помощью нейронных сетей http://matlab.exponenta.ru/conf2002/thesises/section4/sautov/sautov.php

Следователь
22/12/15 16:33

Константин
22/12/15 17:22

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

Следователь
22/12/15 17:27

Константин
22/12/15 17:30

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

DZ
05/10/20 13:05

Боже, что за бред здесь написан?? Автор, похоже, в принципе не представляет работу фильтра Калмана.

Константин
05/10/20 15:59

Источник

Фильтр Калмана — это легко

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

Об алгоритме

Что же нам потребуется для работы фильтра Калмана?

Обычно используются следующие обозначения:

— вектор состояния;
— мера неопределенности вектора состояния. Представляет из себя ковариационную матрицу (об этом позже — это будет, наверное, самая сложная часть).

Содержимое вектора состояния зависит от фантазии разработчика и решаемой задачи. Например, мы можем отслеживать координаты объекта, а также его скорость и ускорение. В этом случае получается вектор из трёх переменных: <позиция, скорость, ускорение> (для одномерного случая; для 3D мира будет по одному такому набору для каждой оси, то есть, 9 значений в векторе)

Математическая модель системы / процесса

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

Фильтр Калмана накладывает ограничения на используемые модели: это должны быть дискретные модели в пространстве состояний. А ещё они должны быть линейными.

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

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

Давайте разберём это уравнение подробно. В первую очередь, нас интересует первое слагаемое () — это как раз модель эволюции процесса. А матрица (также встречаются обозначения , ) — называется матрицей процесса (state transition matrix). Она задаёт систему линейных уравнений, описывающих, как получается новое состояние системы из предыдущего.

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

Первая строка матрицы — хорошо знакомое уравнение . Аналогично, вторая строка матрицы описывает изменение скорости. Третья строка описывает изменение ускорения.

В некоторых случаях, мы напрямую управляем процессом (например, управляем квадракоптером с помощью пульта Д/У) и нам достоверно известны задаваемые параметры (заданная на пульте скорость полёта). Второе слагаемое — это модель управления. Матрица называется матрицей управления, а вектор — вектор управляющих воздействий. В случаях когда мы только наблюдаем за процессом, это слагаемое отсутствует.

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

Модель наблюдения

— это вектор измерения/наблюдения.Это значения, получаемые с датчиков системы.

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

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

Вернёмся к нашему примеру. Пусть у нас на роботе установлен один единственный датчик — GPS приёмник (“измеряет” положение). В этом случае матрица будет выглядеть следующим образом:

Строки матрицы соответствуют переменным в векторе состояния, столбцы — элементам вектора измерений. В первой строке матрицы находится значение “1” так как единица измерения положения в векторе состояния совпадает с единицей измерения значения в векторе измерений. Остальные строки содержат “0” потому что переменные состояния соответствующие этим строкам не измеряются датчиком.

Количество датчиков ничем (кроме здравого смысла) не ограничено.
Например, добавим спидометр:

Второй столбец матрицы соответствует нашему новому датчику.

Несколько датчиков могут измерять один и тот же параметр. Добавим ещё один датчик скорости:

Ковариационные матрицы и где они обитают

Для настройки фильтра нам потребуется заполнить несколько ковариационных матриц:
, и .

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

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

Проиллюстрируем влияние мат. ожидания, дисперсии и ковариации.

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

Чем меньше дисперсия — тем меньше ширина колокола.

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

Ненулевое значение ковариации означает, что существует связь между значениями случайных величин:

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

Рассмотрим каждую матрицу подробнее:

— ковариационная матрица состояния

Квадратная матрица, порядок матрицы равен размеру вектора состояния

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

Во многих случаях нам неизвестны значения ковариации между переменными для изначального состояния (элементы матрицы, расположенные вне главной диагонали). Поэтому можно проигнорировать их, установив равными 0. Фильтр самостоятельно обновит значения в процессе работы. Если же значения ковариации известны, то, конечно же, стоит использовать их.

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

— ковариационная матрица шума измерений

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

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

— ковариационная матрица ошибки модели

Квадратная матрица, порядок матрицы равен размеру вектора состояния.

С этой матрицей обычно возникает наибольшее количество вопросов. Что означает ошибка модели? Каков смысл этой матрицы и за что она отвечает? Как заполнять эту матрицу? Рассмотрим всё по порядку.

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

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

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

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

Допустим, мы отслеживаем перемещение робота, используя модель равноускоренного движения, и вектор состояния содержит следующие переменные: положение x, скорость v и ускорение a. Однако, наша модель не учитывает, что на дороге встречаются неровности.

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

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

Модель непрерывного белого шума

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

В этой модели матрица Q рассчитывается следующим образом

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

— спектральная плотность мощности белого шума

Подставляем матрицу процесса, соответствующую нашей модели:

После перемножения и интегрирования получаем:

Модель “кусочного” белого шума

— мощность шума
— наивысший порядок производной, используемой в модели (т.е. ускорение для вышеописанной модели)

В этой модели матрица определяется следующим образом:

Из матрицы процесса F

берём столбец с наивысшим порядком производной

и подставляем в формулу. В итоге получаем:

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

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

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

Т.е. для нашей модели равноускоренного движения можно взять матрицу следующего вида:

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

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

Рассмотрим вектор состояния

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

Дисперсия, соответствующая наивысшим порядкам производных и , будет находиться на 3-ей и 5-ой позициях на главной диагонали матрицы.

Однако, на практике нет никакого смысла перемешивать порядок переменных состояния таким образом, чтобы порядки производных шли не по очереди — это просто неудобно.

Пример кода

Нет смысла изобретать велосипед и писать свою собственную реализацию фильтра Калмана, когда существует множество готовых библиотек. Я выбрал язык python и библиотеку filterpy для примера.

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

Моделирование данных датчиков

Начнём с равномерного движения:

Визуализируем результаты моделирования:

Реализация фильтра

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

Мы предполагаем, что наш робот находится в точке 0 и имеет нулевые скорость и ускорение в начальный момент времени:

Однако, мы не уверены, что это именно так. Поэтому установим матрицу ковариации для начального состояния с большими значениями на главной диагонали:

Я воспользовался функцией библиотеки filterpy для расчёта ковариационной матрицы ошибки модели: filterpy.common.Q_discrete_white_noise. Эта функция использует модель непрерывного белого шума.

Бонус — сравнение различных порядков моделей

Сравним поведение фильтра с моделями разного порядка. Для начала, смоделируем более сложный сценарий поведения робота. Пусть робот находится в покое первые 20% времени, затем движется равномерно, а затем начинает двигаться равноускоренно:

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

Сравним результаты:

На графиках сразу заметно, что модель первого порядка начинает отставать от истинного значения на участках равномерного движения и равноускоренного движения. Модель второго порядка успешно справляется с участком равномерного движения, но так же начинает отставать на участке равноускоренного движения. Модель третьего порядка справляется со всеми тремя участками.

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

Нелинейные модели и фильтр Калмана

Почему фильтр Калмана не работает для нелинейных моделей и что делать

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

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

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

Unscented Kalman Filter (UKF). Этот подход строит приближение распределения получающегося после нелинейного преобразования при помощи сигма-точек. Преимуществом этого метода является то, что он не требует вычисления производных.

Мы рассмотрим именно Unscented Kalman Filter

Unscented Kalman Filter и почему он без запаха

Основная магия этого алгоритма заключается в методе, который строит приближение распределения плотности вероятности случайной величины после прохождения через нелинейное преобразование. Этот метод называется unscented transform — сложнопереводимое на русский язык название. Автор этого метода, Джеффри Ульман, не хотел, чтобы его разработку называли “Фильтр Ульмана”. Согласно интервью, он решил назвать так свой метод после того как увидел дезодорант без запаха (“unscented deodorant”) на столе в лаборатории, где он работал.

Этот метод достаточно точно строит приближение функции распределения случайной величины, но что более важно — он очень простой.

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

Реализуем unscented Kalman filter для линейной модели из прошлого примера:

Разница в коде минимальна. Мы заменили матрицы F и H на функции f(x) и h(x). Это позволяет использовать нелинейные модели системы и/или наблюдения:

Также, появилась строчка, устанавливающая алгоритм генерации сигма-точек

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

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

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

Такую модель измерений было бы невозможно использовать в случае с линейным фильтром Калмана

Вместо заключения

За рамками статьи остались теоретические основы фильтра Калмана. Однако объем материала по этой теме ошеломляет. Сложно выбрать хороший источник. Я бы хотел рекомендовать замечательную книгу от автора библиотеки filterpy Roger Labbe (на английском языке). В ней доступно описаны принципы работы алгоритма и сопутствующая теория. Книга представляет собой документ Jupyter notebook, который позволяет в интерактивном режиме экспериментировать с примерами.

Источник