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

в 17:25, , рубрики: Алгоритмы, калман, математика, робототехника, теория вероятности, фильтр калмана, метки: ,

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

В интернете, в том числе и на хабре, можно найти много информации про фильтр Калмана. Но тяжело найти легкоперевариваемый вывод самих формул. Без вывода вся эта наука воспринимается как некое шаманство, формулы выглядят как безликий набор символов, а главное, многие простые утверждения, лежащие на поверхности теории, оказываются за пределами понимания. Целью этой статьи будет рассказать об этом фильтре на как можно более доступном языке.
Фильтр Калмана — это мощнейший инструмент фильтрации данных. Основной его принцип состоит в том, что при фильтрации используется информация о физике самого явления. Скажем, если вы фильтруете данные со спидометра машины, то инерционность машины дает вам право воспринимать слишком быстрые скачки скорости как ошибку измерения. Фильтр Калмана интересен тем, что в каком-то смысле, это самый лучший фильтр. Подробнее обсудим ниже, что конкретно означают слова «самый лучший». В конце статьи я покажу, что во многих случаях формулы можно до такой степени упростить, что от них почти ничего и не останется.

Ликбез

Перед знакомством с фильтром Калмана я предлагаю вспомнить некоторые простые определения и факты из теории вероятности.

Случайная величина

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

Довольно часто в жизни случайные величины распределены по Гауссу, когда плотность вероятности равна Фильтр Калмана.
Фильтр Калмана
Мы видим, что функция Фильтр Калмана имеет форму колокола с центром в точке Фильтр Калмана и с характерной шириной порядка Фильтр Калмана.
Раз мы заговорили о Гауссовом распределении, то грешно будет не упомянуть, откуда оно возникло. Также как и числа Фильтр Калмана и Фильтр Калмана прочно обосновались в математике и встречаются в самых неожиданных местах, так и распределение Гаусса пустило глубокие корни в теорию вероятности. Одно замечательное утверждение, частично объясняющее Гауссово всеприсутствие, состоит в следующем:
Пусть есть случайная величина Фильтр Калмана имеющая произвольное распределение (на самом деле существуют некие ограничения на эту произвольность, но они совершенно не жесткие). Проведем Фильтр Калмана экспериментов и посчитаем сумму Фильтр Калмана «выпавших» значений случайной величины. Сделаем много таких экспериментов. Понятно, что каждый раз мы будем получать разное значение суммы. Иными словами, эта сумма является сама по себе случайной величиной со своим каким-то определенным законом распределения. Оказывается, что при достаточно больших Фильтр Калмана закон распределения этой суммы стремится к распределению Гаусса (к слову, характерная ширина «колокола» растет как Фильтр Калмана). Более подробно читаем в википедии: центральная предельная теорема. В жизни очень часто встречаются величины, которые складываются из большого количества одинаково распределенных независимых случайных величин, поэтому и распределены по Гауссу.

Среднее значение

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

Дисперсия

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

Независимые случайные величины

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

Доказательство

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

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

Постановка задачи

Обозначим за Фильтр Калмана величину, которую мы будем измерять, а потом фильтровать. Это может быть координата, скорость, ускорение, влажность, степень вони, температура, давление, и т.д.
Начнем с простого примера, который и приведет нас к формулировке общей задачи. Представьте себе, что у нас есть радиоуправляемая машинка, которая может ехать только вперед и назад. Мы, зная вес машины, форму, покрытие дороги и т.д., расcчитали как контролирующий джойстик влияет на скорость движения Фильтр Калмана.

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

Тогда координата машины будет изменяться по закону:

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

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

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

У нас есть установленный на машинке GPS сенсор, который пытается мерить истинную координату Фильтр Калмана машинки, и, конечно же, не может ее померить точно, а мерит с ошибкой Фильтр Калмана, которая является тоже случайной величиной. В итоге с сенсора мы получаем ошибочные данные:

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

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

Фильтр Калмана    (1)

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

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

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

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

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

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

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

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

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

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

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

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

Доказательство

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

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

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

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

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

Доказательство

Из того что все случайные величины, входящие в выражение для Фильтр Калмана, независимы, следует, что все «перекрестные» члены равны нулю:
Фильтр Калмана
Мы использовали тот факт, что Фильтр Калмана, тогда формула для дисперсии выглядит намного проще: Фильтр Калмана.

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

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

Здесь мы уже пишем выражение для коэффициента Калмана с индексом шага Фильтр Калмана, тем самым мы подчеркиваем, что он зависит от шага итерации.
Подставляем полученное оптимальное значение Фильтр Калмана в выражение для Фильтр Калмана, которую мы минимизировали. Получаем;

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

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

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

Пример

На рекламной картинке вначале статьи отфильтрованы данные с вымышленного GPS сенсора, установленного на вымышленной машине, которая едет равноускоренно c известным вымышленным ускорением Фильтр Калмана.

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

Код на матлабе

clear all;
N=100  % number of samples
a=0.1 % acceleration
sigmaPsi=1
sigmaEta=50;
k=1:N
x=k
x(1)=0
z(1)=x(1)+normrnd(0,sigmaEta);
for t=1:(N-1)
  x(t+1)=x(t)+a*t+normrnd(0,sigmaPsi); 
   z(t+1)=x(t+1)+normrnd(0,sigmaEta);
end;
%kalman filter
xOpt(1)=z(1);
eOpt(1)=sigmaEta;
for t=1:(N-1)
  eOpt(t+1)=sqrt((sigmaEta^2)*(eOpt(t)^2+sigmaPsi^2)/(sigmaEta^2+eOpt(t)^2+sigmaPsi^2))
  K(t+1)=(eOpt(t+1))^2/sigmaEta^2
 xOpt(t+1)=(xOpt(t)+a*t)*(1-K(t+1))+K(t+1)*z(t+1)
end;
plot(k,xOpt,k,z,k,x)

Анализ

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

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

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

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

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

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

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

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

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

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

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

Обсуждение

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

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

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

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

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

Многомерный случай

Всю теорию фильтра Калмана можно обобщить на многомерный случай. Формулы там выглядят чуть страшнее, но сама идея их вывода такая же, как и в одномерном случае. В этой прекрасной статье вы можете увидеть их: http://habrahabr.ru/post/140274/.
А в этом замечательном видео разобран пример, как их использовать.

Литература

Оригинальную статью Калмана можно скачать вот тут: http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf.

Автор: khdavid

Источник

Поделиться

* - обязательные к заполнению поля