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

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

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

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


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

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

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



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


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



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

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

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





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

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


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



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

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

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

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

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

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

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

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

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

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

        class KalmanFilterSimple1D
        {
            public double X0 {get; private set;} // predicted state
            public double P0 { get; private set; } // predicted covariance
    
            public double F { get; private set; } // factor of real value to previous real value
            public double Q { get; private set; } // measurement noise
            public double H { get; private set; } // factor of measured value to real value
            public double R { get; private set; } // environment noise
    
            public double State { get; private set; }
            public double Covariance { get; private set; }
    
            public KalmanFilterSimple1D(double q, double r, double f = 1, double h = 1)
            {
                Q = q;
                R = r;
                F = f;
                H = h;
            }
    
            public void SetState(double state, double covariance)
            {
                State = state;
                Covariance = covariance;
            }
    
            public void Correct(double data)
            {
                //time update - prediction
                X0 = F*State;
                P0 = F*Covariance*F + Q;
    
                //measurement update - correction
                var K = H*P0/(H*P0*H + R);
                State = X0 + K*(data - H*X0);
                Covariance = (1 - K*H)*P0;            
            }
        }
    
    
    
        // Применение...
    
        var fuelData = GetData();
        var filtered = new List<double>();
    
        var kalman = new KalmanFilterSimple1D(f: 1, h: 1, q: 2, r: 15); // задаем F, H, Q и R
        kalman.SetState(fuelData[0], 0.1); // Задаем начальные значение State и Covariance
        foreach(var d in fuelData)
        {
            kalman.Correct(d); // Применяем алгоритм
    
            filtered.Add(kalman.State); // Сохраняем текущее состояние 
        }
    


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



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

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

    UPD: Список источников:
    CS373 — PROGRAMMING A ROBOTIC CAR — очень рекомендую
    Википедия (русская)
    Википедия (англ.)
    На хабре: 1 и 2

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

    Подробнее
    Реклама
    Комментарии 47
    • –9
      А поиск пробовали использовать?
      habrahabr.ru/post/120133/
      habrahabr.ru/post/121904/
      Даже на Википедии более подробное описание и больше по ссылкам найти можно — ru.wikipedia.org/wiki/%D0%A4%D0%B8%D0%BB%D1%8C%D1%82%D1%80_%D0%9A%D0%B0%D0%BB%D0%BC%D0%B0%D0%BD%D0%B0
      • +34
        Фильтра Калмана много не бывает
        • 0
          Спасибо за ссылки! Теперь прочитаю 3 статьи.
          • 0
            Зато с кодом)

          • +8
            Это самая адекватная и самая понятная статья из всех что есть на Хабре. Спасибо огромное, мне он как раз сейчас нужен.
            • +3
              Завидую! У меня низкий порог вхождения закончился на второй картинке…
            • +5
              Отличный способ подачи материала. Отличнейшая статья.
              • +7
                Тем временем в курсе на Udacity в третьем разделе есть то же самое, только на питоне и с примерами для 1- 2- и 4-мерных состояний
                • +1
                  Во втором только. :)
                  • 0
                    Именно посмотрев этот курс, разобрался что это за фильтр ) Рекомендую к просмотру — отличный курс, добавлю ссылку в статью, как только доберусь до ноута. Спасибо, что напомнили… постил уже почти ночью, забыл про ссылки.
                  • +5
                    А если сравнить со скользящей средней, намного лучше?)
                    • +2
                      В данном конкретном случае — нет, он вырождается в автокорреляцию. Но вообще это крайне мощная штука, которая подстраивает модель объекта под реальный объект.

                      Вся фишка фильтра Калмана в том, что он:
                      — позволяет учитывать зависимости между параметрами (матрица F в предсказании)
                      — позволяет учитывать внешнее воздействие на систему (матрица B в предсказании)
                      — автоматически подбирает усиление в зависимости от ошибки прогноза, причём это хорошо работает даже в присутствии шума
                      — ну и, как уже упоминалось в заключении статьи, можно получать сведения о внутренних параметрах системы, которые, по какой либо причине, невозможно измерить.
                    • 0
                      udacity.com — Programming a Robotic Car — довольно понятное объяснение объяснение фильтра Калмана и лабораторная работа по программированию его.
                      • 0
                        ну и на хабре еще была статейка habrahabr.ru/post/120133/, но эта попонятнее будет как по мне, за что спасибо автору!
                        • 0
                          В моей статье сделан упор на практику построения ФК. Здесь этого нет. Каков физический смысл матриц слишком «долго и нудно объяснять». А так… да. Все просто! Спасибо автору!
                      • 0
                        Че-то у меня не взлетело с ходу:

                        Реагирует только на параметры H (фактически масштабирует входной сигнал) и F (работает как размер окна в running average)
                        • 0
                          H в единицу ставьте, и поиграйтесь с параметрами Q и R в сторону увеличения. Я правильно понимаю, что измеряемая величина — красный график, а выход фильтра — синий? Тогда видно, что фильтр работает, но измеряется сигнал с намного бОльшим шумом, чем ожидает фильтр.
                          • 0
                            > и поиграйтесь с параметрами Q и R в сторону увеличения
                            Важнее не их абсолютные значения, а отношение одного к другому. Q << R => Сильное сглаживание и высокая инерционность. Q ~ R => Сглаживания почти не будет, зато при большой изменчивости полезного сигнала фильтр не будет вносить искажений АЧХ.
                            • 0

                              Вот же ж блин, а
                              • 0
                                хм, интересно, а вот народ на леталках типа квадрокомперов — подключают данные с приемника чтобы управляющие воздействия отслеживать?
                          • 0
                            А чем вы выводите график на Tk?
                            • 0
                              Листы отрезков. При обновлении берем, дергаем первый отрезок из списка, вставляем в конец с новыми координатами.
                              Подтормаживает.
                          • 0
                            Спасибо, для начинающих очень хорошо изложена суть =)) Ждем новых статей =)
                            • 0
                              > H — матрица определяющая отношение между измерениями и состоянием системы, пока без объяснений примем эту переменную также равную 1.
                              Как это матрица равна скаляру? И почему 1, а не «100500»?
                              • 0
                                Сам себе выкопал яму решив упростить до скаляров, заодно создав путаницу в названиях. В данном случае это матрица состоящая из одного элемента. Равна 1 потому, что измерения накладываются на состояние как есть без преобразования.

                                У вас более глубокая статья, но нужно знать алгоритм прежде чем применять его. Возможно сказывается слабое математического образования, но сходу я не разобрался, что же там написано…
                                • 0
                                  Про вычисление ковариаций напишите?
                              • 0
                                Очень полезная статья! Надеюсь найти в следующей пример замешивания информации с разных источников данных. Напимер: гироскопы + акселерометры + магнитные датчики. Ну и естественно пример расчёта матриц H и Q.
                                • 0
                                  H — матрица измерений. Она строится элементарно по модели датчика.

                                  > пример расчёта матриц H и Q
                                  Имелось в виду R и Q? Если да, то выше зареквестил. С R ничего сложного — просто дисперсии и корреляции между выходными сигналами датчиков. А вот Q нельзя точно измерить — это внутри датчика. Можно знать примерно.
                                  Я же использую Q для оптимизации ФК.
                                  • 0
                                    Нигде не встречал какой-то конкретной методики нахождения Q. Это даже не внутри датчика, а внутри процесса.

                                    Видимо это классическая задача поиска, можно попробовать применить логистическую регрессию — то есть проводим серию экспериментов, со знанием реальных значений… и потом зная реальные значения и показания датчика используем градиентный спуск (или что-то более продвинутое) для минимизации ошибки которую дает фильтр.
                                    • 0
                                      > Это даже не внутри датчика, а внутри процесса.
                                      А это не одно и то же? Внутри датчика чувствительный элемент (например, массивный элемент со смещенным Ц.М. у акселерометров), который выполнен неидеально. Он участвует в процессе измерения входного воздействия. Инструментальные погрешности, вызывающие неидеальное движение Ч.Э и входят в погрешности динамического процесса.

                                      > Нигде не встречал какой-то конкретной методики нахождения Q.
                                      Для датчиков из среднего и высокого ценового диапазонов производитель по результатам сертификационных и своих внутренних испытаний может иметь приблизительный диапазон значений инструментальных погрешностей. Не каждый производитель и не для каждого прибора их даст (комм. тайна).
                                • НЛО прилетело и опубликовало эту надпись здесь
                                  • 0
                                    Не проще ли сделать дополнительный эталонный резервуар и мерить в нем диэлектрическую проницаемость в данный момент времени?
                                    • 0
                                      Программное решение обычно, как и в данном случае, универсальнее и дешевле. К тому же, насколько я знаю, диэелектрическую проницаемость нельзя измерить напрямую — только косвенно, рассчитав на основе других измерений. Ну и, самое интересное — а погрешность измерения ДП вы не учитываете?

                                      Вы прям напомнили анекдот про сферического коня в вакууме (:
                                      • 0
                                        Тут можно поспорить. Для аппаратного решения достаточно измерить электрическую емкость эталонного емкости с топливом. В тоже время, для программного решения нужны вычислительные мощности.

                                        Несомненно, в любом случае выходной сигнал будет зашумлен, но ошибка будет обусловлена только погрешностью измерений. Таким образом, как раз мой вариант будет ближе к реальности.
                                        • 0
                                          Вряд ли.
                                          Вы учитываете только т.н. коррелирующую составляющую систематической и случайной погрешности датчика. Но есть и некоррелирующая, которая обычно достаточно велика.
                                          А вот фильтр Калмана может отсеять всю случайную составляющую и выделить систематику.
                                          Конечно, при этом важна модель, но для датчика она с хорошей точность довольно проста.
                                    • 0
                                      Очень доходчиво и ясно изложено, пишите ещё, таких авторов надо побольше!
                                      • 0
                                        Жаль, что Вы не упомянули об основе фильтра Калмана — методе наименьших квадратов.
                                        Мне кажется, это упростило бы понимание и отделило основу от конкретики примера.
                                        К тому же не всегда необходим именно фильтр Калмана.
                                        Для случая обработки «постфактум» достаточно гораздо более простого метода наименьших квадратов.
                                        • 0
                                          Думал сначала начать с теории — получилось очень раздуто и запутано, к тому же ещё не до конца понял всего что там происходит, решил не писать про то что сам не до конца понимаю. Будет здорово, если вы раскроете эту тему!
                                          • 0
                                            > более простого метода наименьших квадратов
                                            Есть замечательный труд «Браммер К., Зиффлинг Г. Фильтр Калмана-Бьюси. Пер. с нем. – М.: Наука. Главная
                                            редакция физико-математической литературы. 1982.»
                                            В нем написано, почему МНК (вернее Гауссовский Метод Наименьших Квадратов) на практике не может быть использован в виду сильных ограничений на гипотезу о параметрах помех. Это должен быть белый шум с нулевым средним, причем шумы в разных каналах должны иметь нулевую корреляцию. Такого на практике нет.

                                            Если сигнал один, то МНК не нужен (он многомерный, если говорить именно о Гауссовском). А если сигналов несколько, то почти всегда есть и корреляция и, особенно, смещение среднего, да и белым шумом помехи являются очень редко (в Волоконно-Оптических Гироскопах почти белый).

                                            Я могу написать свои мысли на эту тему, если интересно…
                                            • 0
                                              С этой книгой я знаком, правда много воды утекло с тех пор :)
                                              МНК мы вполне удачно юзали на испытаниях гироскопов, а вот фильтр Калмана существенно сложнее и выигрыша не дал.
                                              Возражения здесь следующие.
                                              1. Многомерность: она возникает, если обрабатывать сигнал даже с одного датчика, но не в реальном времени, а с меньшей дискретизацией, чем идет съем сигнала. Или вообще постфактум.
                                              2. Смещение среднего:
                                              2.1. Есть небольшая модификация для МНК со смещением среднего.
                                              2.2. Можно пользоваться и обычным МНК, только поправить неверную модель, что сделать для смещения совсем несложно.
                                              3. В небелом шуме есть коррелирующая составляющая, чаще всего в ней можно выделить гармоники.
                                              • 0
                                                На самом-то деле гМНК это же частный случай ФК (по сути, хотя хронологически вроде бы наоборот). Есть продолжение гМНК для случая корреляций между каналами — исследовал как раз в сравнении с ФК. Но выигрыш ФК именно в функциональности и гибкости. Можно миксовать показания инерциальных датчиков с GPS, к примеру. А это совершенно разные источники информации как по природе, так и по характерным частотам выдачи информации. Да и инфа от инерциалки есть с большой вероятностью всегда, а GPS очень ненадежный источник. И вот тут ФК и нужен. Через матрицы ковариаций можно временно или перманентно выключать «дескридитированный» канал, давая ему минимальный вес.

                                                > Многомерность: она возникает, если обрабатывать сигнал даже с одного датчика, но не в реальном времени, а с меньшей дискретизацией, чем идет съем сигнала.
                                                Т.е. датчик выдает с частотой 1кГц -> формируем выборку из 10 отсчетов -> из гМНК инфа выходит на частоте 100 Гц. Так? Но с таким же успехом плавающее среднее можно использовать (или полиномом усреднять). Насколько мне известно такой подход приводит к увеличению смещения шума, т.к. осреднение по малой выборке дает не истинное среднее. И гМНК в таком режиме выполняет по сути осреднение. Метод Взвешенных Наименьших Квадратов (с матрицами ковариаций) — это осреднение с весами.
                                          • 0
                                            Отличный стиль изложения и интересный материал. Нравится, что есть и теория и практика, только как раз не хватает того, о чем вы и написали :)

                                            Очень хотелось бы увидеть продолжение.
                                            • +1
                                              Covariance = (1 — K*H)*F;


                                              Судя по картинкам сверху не F, а P0.
                                              • 0
                                                Благодарю! Действительно была опечатка!
                                              • 0
                                                Спасибо за статью. Есть пара замечаний.
                                                1) Крайне удивило отсутствие описания модели фильтруемого процесса.
                                                2) «Error covariance» переведено как «ошибка ковариации» (на картинках-схемках), в то время как верно будет «ковариация ошибки».
                                                • 0
                                                  И ещё небольшое уточнение.
                                                  В цикле foreach(var d in fuelData) нужно исключать fuelData[0], т.е. начинать с fuelData[1], так как fuelData[0] использовалось в качестве начального значения. В данном примере это никакой роли не играет, так как F = 1.

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