1

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

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

навигация

беспилотный летательный аппарат (БПЛА)

1. Макаренко Г.К., Алешечкин А.М. Исследование алгоритма фильтрации при определении координат объекта по сигналам спутниковых радионавигационных систем // Доклады ТУСУРа. – 2012. – № 2 (26). – С. 15-18.

2. Bar-Shalom Y., Li X. R., Kirubarajan T. Estimation with Applications

to Tracking and Navigation // Theory Algorithms and Software. – 2001. – Vol. 3. – P. 10-20.

3. Bassem I.S. Vision based Navigation (VBN) of Unmanned Aerial Vehicles (UAV) // UNIVERSITY OF CALGARY. – 2012. – Vol. 1. – P. 100-127.

4. Conte G., Doherty P. An Integrated UAV Navigation System Based on Aerial Image Matching // Aerospace Conference. – 2008. –Vol. 1. – P. 3142-3151.

5. Guoqiang M., Drake S., Anderson B. Design of an extended kalman filter for uav localization // In Information, Decision and Control. – 2007. – Vol. 7. – P. 224–229.

6. Ponda S.S Trajectory Optimization for Target Localization Using Small Unmanned Aerial Vehicles // Massachusetts institute of technology. – 2008. – Vol. 1. – P. 64-70.

7. Wang J., Garrat M., Lambert A. Integration of gps/ins/vision sensors to navigate unmanned aerial vehicles // IAPRS&SIS. – 2008. – Vol. 37. – P. 963-969.

Одной из актуальных задач современной навигации беспилотных летательных аппаратов (БПЛА) является задача повышения точности определения координат. Эта задача решается путём использования различных вариантов комплексирования навигационных систем. Одним из современных вариантов комплексирования является сочетание gps/глонасс-навигации с расширенным фильтром Калмана (Extended Kalmanfilter), рекурсивно оценивающего точность с помощью неполных и зашумленных измерений. В данный момент существуют и разрабатываются различные вариации расширенного фильтра Калмана, включающие разнообразное число переменных состояний . В этой работе мы покажем, насколько эффективным может быть его использование в современных разработках. Рассмотрим одно из характерных представлений подобного фильтра .

Построение математической модели

В данном примере мы будем говорить только о движении БПЛА в горизонтальной плоскости, иначе, мы рассмотрим так называемую проблему 2d локализации . В нашем случае это оправдано тем, что для многих практически встречающихся ситуаций БПЛА может оставаться примерно на одной и той же высоте. Это предположение широко используется для упрощения моделирования динамики летательных аппаратов . Динамическая модель БПЛА задается следующей системой уравнений:

где {} - координаты БПЛА в горизонтальной плоскости как функции времени, направление БПЛА, угловая скорость БПЛА, и vпутевая скорость БПЛА, функции и будем считать постоянными. Они взаимно независимы, с известными ковариациями и , равными и соответственно и используются для моделирования изменений ускорения БПЛА, вызванных ветром, маневрами пилота и т.д. Значения и являются производными от максимальной угловой скорости БПЛА и опытных значений изменений линейной скорости БЛА, - символ Кронекера.

Данная система уравнений будет приближенной из-за нелинейности в модели и из-за присутствия шума. Самый простой способ аппроксимации в данном случае - это приближение методом Эйлера. Дискретная модель динамической системы движения БПЛА показана ниже.

дискретный вектор состояний фильтра Калмана, позволяющий аппроксимировать значение непрерывного вектора состояний. ∆ - временной интервал между k и k+1 измерениями. {} и {} - последовательности значений белого гауссовского шума с нулевым средним значением. Матрица ковариации для первой последовательности:

Аналогично, для второй последовательности:

Выполнив соответствующие замены в уравнениях системы (2), получаем:

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

(3)

Уравнение для :

= + , (4)

где, х и y - координаты БПЛА в k-момент времени, а гауссовская последовательность случайных параметров с нулевым средним значением, которая используется для задания погрешности. Предполагается, что эта последовательность не зависит от {} и {}.

Выражения (3) и (4) служат основой для оценки местоположения БПЛА, где к-е координаты получены с помощью расширенного фильтра Калмана. Моделлирование отказа навигационных систем применительно к данному типу фильтра показывает его существенную эффективность .

Для большей наглядности приведем небольшой простой пример. Пусть некоторый БПЛА летит равноускоренно, с некоторым постоянным ускорением а.

Где, х - координата БПЛА в t-момент времени, а δ - некоторая случайная величина.

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

Рис. 1. Фильтрация показания сенсора с помощью фильтра Калмана

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

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

Частичный фильтр

Представим один из алгоритмов, развивающих идеи расширенного фильтра Калмана - частичный фильтр. Частичная фильтрация является неоптимальным способом фильтрации, который работает при выполнении объединения методом Монте-Карло на множестве частиц, которые представляют собой распределение вероятностей процесса. Здесь частица это элемент, взятый из априорного распределения оцениваемого параметра. Основная идея частичного фильтра заключается в том, что большое количество частиц может быть использовано для представления оценки распределения. Чем большее число частиц используется, тем точнее множество частиц будет представлять априорное распределение. Фильтр частиц инициализируется помещением в него N частиц из априорного распределения параметров, которые мы хотим оценить. Алгоритм фильтрации предполагает прогон этих частиц через специальную систему, а затем взвешивание с помощью информации, полученной от измерения данных частиц. Полученные частицы и связанные с ними массы представляют апостериорное распределение оценочного процесса. Цикл повторяется для каждого нового измерения, и веса частиц обновляются для представления последующего распределения. Одна из основных проблем с традиционным подходом фильтрации частиц состоит в том, что в результате такой подход обычно имеет несколько частиц, имеющих очень большой вес, в отличие от большинства остальных, вес которых очень незначителен. Это приводит к нестабильности фильтрации . Эта проблема может быть решена введением частоты дискретизации, где N новых частиц берется из распределения, составленного из старых частиц. Результат оценки получают путем получения выборки среднего значения множества частиц. Если мы имеем несколько независимых выборок, то средняя выборка будет точной оценкой среднего значения, задающей конечную дисперсию.

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

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

Перспективные исследования в данной области

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

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

Заключение

В заключение отметим, что использование фильтра Калмана в системах определения местоположения БПЛА практикуется во многих современных разработках. Существует огромное количество вариаций и аспектов такого использования, вплоть до одновременного применения нескольких подобных фильтров с разными факторами состояний . Одним из наиболее перспективных направлений развития Калмановских фильтров видится работа над созданием модифицированного фильтра, погрешности которого будут представлены цветным шумом, что сделает его еще более ценным для решения реальных задач. Также большой интерес в данной области представляет собой частичный фильтр, с помощью которого можно фильтровать негауссовские шумы. Названное разнообразие и ощутимые результаты в повышении точности, особенно в случае отказа стандартных спутниковых навигационных систем, являются главными факторами влияния данной технологии на различные научные области, связанные с разработкой точных и отказоустойчивых навигационных систем для различных летательных аппаратов.

Рецензенты:

Лабунец В.Г., д.т.н., профессор, профессор кафедры теоретических основ радиотехники Уральского федерального университета имени первого Президента России Б.Н. Ельцина, г. Екатеринбург;

Иванов В.Э., д.т.н., профессор, зав. кафедрой технологии и средств связи Уральского федерального университета имени первого Президента России Б.Н. Ельцина, г. Екатеринбург.

Библиографическая ссылка

Гаврилов А.В. ИСПОЛЬЗОВАНИЕ ФИЛЬТРА КАЛМАНА ДЛЯ РЕШЕНИЯ ЗАДАЧ УТОЧНЕНИЯ КООРДИНАТ БПЛА // Современные проблемы науки и образования. – 2015. – № 1-1.;
URL: http://science-education.ru/ru/article/view?id=19453 (дата обращения: 01.02.2020). Предлагаем вашему вниманию журналы, издающиеся в издательстве «Академия Естествознания»

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

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

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

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

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

Введение

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

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

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

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

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

• Расширенный фильтр Калмана (EKF, Extended Kalman filter). Сведение нелинейных моделей наблюдений и формирующего процесса с помощью линеаризации посредством разложения в ряд Тейлора .
• Unscented Kalman filter (UKF). Используется в задачах, в которых простая линеаризация приводит к уничтожению полезных связей между компонентами вектора состояния. В этом случае «линеаризация» основана на unscented -преобразовании.
• Ensemble Kalman filter (EnKF). Используется для уменьшения размерности задачи.
• Возможны варианты с нелинейным дополнительным фильтром, позволяющим привести негауссовские наблюдения к нормальным.
• Возможны варианты с «обеляющим» фильтром, позволяющим работать с «цветными» шумами
• и т. д.

Используемая модель динамической системы

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

При использовании фильтра Калмана для получения оценок вектора состояния процесса по серии зашумленных измерений необходимо представить модель данного процесса в соответствии со структурой фильтра - в виде матричного уравнения определенного типа. Для каждого такта k работы фильтра необходимо в соответствии с приведенным ниже описанием определить матрицы: эволюции процесса F k ; матрицу наблюдений H k ; ковариационную матрицу процесса Q k ; ковариационную матрицу шума измерений R k ; при наличии управляющих воздействий - матрицу их коэффициентов B k .

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

Модель системы/процесса подразумевает, что истинное состояние в момент k получается из истинного состояния в момент k −1 в соответствии с уравнением:

,

• F k - матрица эволюции процесса/системы, которая воздействует на вектор x k −1 (вектор состояния в момент k −1 );
• B k - матрица управления, которая прикладывается к вектору управляющих воздействий u k ;
• w k - нормальный случайный процесс с нулевым математическим ожиданием и ковариационной матрицей Q k , который описывает случайный характер эволюции системы/процесса:

В момент k производится наблюдение (измерение) z k истинного вектора состояния x k , которые связаны между собой уравнением:

где H k - матрица измерений, связывающая истинный вектор состояния и вектор произведенных измерений, v k - белый гауссовский шум измерений с нулевым математическим ожиданием и ковариационной матрицей R k :

Начальное состояние и векторы случайных процессов на каждом такте {x 0 , w 1 , …, w k , v 1 , …, v k } считаются независимыми .

Многие реальные динамические системы нельзя точно описать данной моделью. На практике неучтённая в модели динамика может серьёзно испортить рабочие характеристики фильтра, особенно при работе с неизвестным стохастическим сигналом на входе. Более того, неучтённая в модели динамика может сделать фильтр неустойчивым . С другой стороны, независимый белый шум в качестве сигнала не будет приводить к расхождению алгоритма. Задача отделения шумов измерений от неучтенной в модели динамики сложна, решается она с помощью теории робастных систем управления .

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

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

Состояние фильтра задается двумя переменными:

Итерации фильтра Калмана делятся на две фазы: экстраполяция и коррекция. Во время экстраполяции фильтр получает предварительную оценку состояния системы (в русскоязычной литературе часто обозначается , где означает «экстраполяция», а k - номер такта, на котором она получена) на текущий шаг по итоговой оценке состояния с предыдущего шага (либо предварительную оценку на следующий такт по итоговой оценке текущего шага, в зависимости от интерпретации). Эту предварительную оценку также называют априорной оценкой состояния, так как для её получения не используются наблюдения соответствующего шага. В фазе коррекции априорная экстраполяция дополняется соответствующими текущими измерениями для коррекции оценки. Скорректированная оценка также называется апостериорной оценкой состояния, либо просто оценкой вектора состояния . Обычно эти две фазы чередуются: экстраполяция производится по результатам коррекции до следующего наблюдения, а коррекция производится совместно с доступными на следующем шаге наблюдениями, и т. д. Однако возможно и другое развитие событий, если по некоторой причине наблюдение оказалось недоступным, то этап коррекции может быть пропущен и выполнена экстраполяция по нескорректированной оценке (априорной экстраполяции). Аналогично, если независимые измерения доступны только в отдельные такты работы, всё равно возможны коррекции (обычно с использованием другой матрицы наблюдений H k ).

Этап экстраполяции

Этап коррекции

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

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

Инварианты

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

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

где - математическое ожидание .

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

Пример построения фильтра

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

В данной задаче матрицы F , H , R и Q не зависят от времени, опустим их индексы. Кроме того, мы не управляем вагонеткой, поэтому матрица управления B отсутствует.

Координата и скорость вагонетки описывается вектором в линейном пространстве состояний

где - скорость (первая производная координаты по времени).

Будем считать, что между (k −1 )-ым и k -ым тактами вагонетка движется с постоянным ускорением a k , распределенным по нормальному закону с нулевым математическим ожиданием и среднеквадратическим отклонением σ a . В соответствии с механикой Ньютона можно записать

.

Ковариационная матрица случайных воздействий

(σ a - скаляр).

На каждом такте работы производится измерение положения вагонетки. Предположим, что погрешность измерений v k имеет нормальное распределение с нулевым математическим ожиданием и среднеквадратическим отклонением σ z . Тогда

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

.

Начальное положение вагонетки известно точно

, .

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

, .

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

Вывод формул

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

По определению ковариационной матрицы P k |k

подставляем выражение для оценки вектора состояния

и расписываем выражение для вектора ошибок

и вектора измерений

выносим вектор погрешности измерений v k

так как вектор погрешности измерений v k не коррелирован с другими аргументами, получаем выражение

в соответствии со свойствами ковариации векторов данное выражение преобразуется к виду

заменяя выражение для ковариационной матрицы экстраполяции вектора состояния на P k |k −1 и определение ковариационной матрицы шумов наблюдений на R k , получаем

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

Оптимальная матрица коэффициентов усиления

Фильтр Калмана минимизирует сумму квадратов математических ожиданий ошибок оценки вектора состояния.

Вектор ошибки оценки вектора состояния

Стоит задача минимизировать сумму математических ожиданий квадратов компонент данного вектора

,

что эквивалентно минимизации следа ковариационной матрицы оценки вектора состояния P k |k . Подставим в выражение для ковариационной матрицы оценки вектора состояния имеющиеся выражения и дополним до полного квадрата:

Заметим что, последнее слагаемое является ковариационной матрицей некоторой случайной величины, поэтому его след неотрицателен. Минимум следа достигнется при обнулении последнего слагаемого:

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

Ковариационная матрица оценки вектора состояния при использовании оптимальной матрицы коэффициентов

Выражение для ковариационной матрицы оценки вектора состояния P k |k при использовании оптимальной матрицы коэффициентов примет вид:

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

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

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

На этапе предсказания происходит:

1. Предсказание состояния системы:

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

1. Предсказание ошибки ковариации:

где – предсказание ошибки; – ошибка в предыдущий момент времени; – ковариация шума процесса.

На этапе корректировки происходит:

1. Вычисление усиления Калмана:

где – усиление Калмана; – матрица измерений, отображающая отношение измерений и состояний; – ковариация шума измерения.

где – измерение в текущий момент времени.

1. Обновление ошибки ковариации:

где – матрица идентичности.

Если состояние системы описывается одной переменной, то = 1, а матрицы вырождаются в обычные уравнения.

Чтобы наглядно продемонстрировать эффективность фильтра Калмана, был проведён эксперимент с датчиком громкости AVR PIC KY-037, который подключен к микроконтроллеру Arduino Uno. На рисунке 1 представлен график показаний датчика без применения фильтра (линия 1). Хаотичные колебания значения на выходе датчика свидетельствуют о наличии шумов.

Рисунок 1. График показаний датчика без применения фильтра

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

Расчёт переменной произведём в программе Microsoft Excel 2010. Для этого необходимо рассчитать среднеквадратичное отклонение для выборки значений показаний датчика. = 0,62. подбирается в зависимости от требуемого уровня фильтрации, принимаем = 0,001. На рисунке 2 второй линией представлен график показаний датчика с применением фильтра.

Рисунок 2. График показаний датчика с применением фильтра Калмана

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

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

Рисунок 3. Реакция фильтра Калмана на скачок измеряемой величины

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

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

Список литературы:

1. ГОСТ 17657-79. Передача данных. Термины и определения. – Москва: Изд-во стандартов, 2005. – 2 с.
2. Фильтр Калмана // Википедия. . Дата обновления: 26.04.2017. URL: http://ru.wikipedia.org/?oldid=85061599 (дата обращения: 21.05.2017).

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

Ликбез

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

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

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

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

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

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

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

Дисперсия

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

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

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

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

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

Из этого сразу же следует, что:

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Мы использовали тот факт, что , тогда формула для дисперсии выглядит намного проще: .

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

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

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

Пример

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

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/ .
А в этом замечательном видео разобран пример, как их использовать.

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

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

Начнем с небольшого примера. Пусть перед нами стоит задача определять координату летящего самолета. Причем, естественно, координата (обозначим ее ) должна определяться максимально точно.

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

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

Задача поставлена, перейдем к ее решению.

Пусть мы знаем управляющее воздействие (), благодаря которому летит самолет (пилот сообщил нам, какие рычаги он дергает 😉). Тогда, зная координату на k-ом шаге, мы можем получить значение на (k+1) шаге:

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

где – ошибка, вызванная внешним воздействием, неидеальностью двигателя итп.

Итак, что же получается? На шаге (k+1) мы имеем, во-первых, неточное показание датчика , а во-вторых, неточно рассчитанное значение , полученное из значения на предыдущем шаге.

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

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

А теперь предположим, что связи с пилотом нет, и мы не знаем управляющее воздействие . Казалось бы, в этом случае фильтр Калмана мы использовать не можем, но это не так 😉 Просто “выкидываем” из формулы то, что мы не знаем, тогда

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

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

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