Фильтр калмана что это такое

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

Фильтр Калмана — это, наверное, самый популярный алгоритм фильтрации, используемый во многих областях науки и техники. Благодаря своей простоте и эффективности его можно встретить в 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)

Давайте подробно обсудим, что нам известно:

Алгоритм Калмана

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

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

Распишем последнее выражение:

Это выражение принимает минимальное значение, когда (приравниваем производную к нулю)

Наша задача решена. Мы получили итерационную формулу, для вычисления коэффициента Калмана.

Пример

Анализ

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

Второй пример

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

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

Обсуждение

Как мы увидели, основная идея фильтра Калмана состоит в том, что надо найти коэффициент такой, чтобы отфильтрованное значение

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

Источник

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

Фильтр Калмана — это, наверное, самый популярный алгоритм фильтрации, используемый во многих областях науки и техники. Благодаря своей простоте и эффективности его можно встретить в 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-координаты, угол поворота руля, обороты двигателя… и все это даст повышение точности координат.

Источник

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

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

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

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

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

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

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

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

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

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

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

Допустим, наш робот имеет состояние , то есть просто позицию и вектор скорости:

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Расширенную ковариацию мы получим, просто прибавив , что даст нам полное выражение шага прогнозирования:

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

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

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

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

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

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

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

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

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

Мы обозначим ковариацию этой неопределённости (например, шум датчиков) как . Распределение имеет среднее значение, равное наблюдаемому показанию, которое мы назовём .

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

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

Каким же будет наиболее вероятное новое состояние? Для каждого возможного показания есть две связанные с ним вероятности: (1) вероятность того, что наше показание датчика является (ошибочным) измерением и (2) вероятность того, что наше предыдущее возможное значение считает показанием, которое мы должны увидеть.

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

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

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

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

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

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

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

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

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

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

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

— это матрица, называемая коэффициентом усиления Калмана; скоро мы её используем.

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

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

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

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

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

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

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

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

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

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

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

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

Источник