Фильтр калмана для чайников

Фильтр калмана для чайников

Жена посылает мужа-программиста в магазин и говорит, купи батон колбасы, а если будут яйца — возьми десяток. Он в магазине: У Вас яйца есть? -Есть -Тогда дайте десять батонов колбасы..

Pages

Thursday, June 16, 2011

Фильтр Калмана, руководство полного идиота

Фильтру Калмана посвящено множество статей и книг, совсем недавно была опубликована статья на хабре. Однако, чтение подобных статей у меня всегда опасение: а не идиот ли я? К сожалению, разобраться в чем же собственно суть дела, за всеми этими многоэтажными индексами и закорючками довольно сложно. Да и нужно ли? Здесь я не буду в очередной раз расписывать алгоритм и его математические основы, об этом можно почитать в книжках, википедии или где угодно еще. Сегодня я постараюсь изложить практическую сторону вопроса.

Итак, что же такое фильтр Калмана. Предположим у вас есть некая величина X изменяющаяся во времени. У вас есть некий прибор, измеряющий величину X с некоторой погрешностью. Вы хотите оценить, чему на самом деле равна величина X. Например, в случае акций Xk может быть истинной ценой акции в момент времени k (она определяется доходами компании, курсом валют, ожиданиями инвесторов итп). У нас есть только один способ измерить эту величину — пойти на биржу и посмотреть котировки. Но котировки на бирже подвержены влиянию массы факторов, цены колеблются в широком диапазоне и измеренная величина Yk может отличаться от Xx. Алгоритм Калмана позволяет нам оценить истинное значение Xk на основе истории изменения измеряемой величины Yk. При этом, естественно, делается ряд предположений о связи между Y и X и характере изменения X.

Предположение номер 1

Т.е. величина X в момент времени k представляет собой линейную комбинацию величины X в прошлый момент времени, управляющего сигнала U и шума W. В реальных задачах часто управляющий сигнал U отсутствует.

Что такое управляющий сигнал? Вернемся к нашему примеру с акциями. Предположим, что ФРС США активно печатает деньги, например 600 млрд долларов, эти деньги льются широкой рекой на рынок и неизбежно двигают цены акций вверх. Это и есть управляющий сигнал.

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

Предположение номер 2

Т.е. наша измеряемая величина Y это линейная комбинация реальной величины X и погрешности измерения V. При этом, делается дополнительное предположение что погрешность измерения тоже имеет нормальное распределение со своей дисперсией dV. Очень часто на практике H = 1.

Что мы только что сделали? Мы построили модель, описывающую нашу систему, при этом, заметьте, пока никакого фильтра Калмана не упоминалось.

Что мы хотим сделать?

Мы хотим получить некое уравнение

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

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

Где Kk — это Калмановский коэффициент усиления. Именно эту величину, в процессе работы и определяет алгоритм фильтра Калмана. При этом фильтр Калмана находит оптимальную величину коэффициента усиления при заданных дисперсиях dW и dV. Чем лучше вы оцените эти дисперсии, тем лучше будет работать фильтр Калмана.

А теперь, представьте, что X и Y это не одномерные величины, а вектор. A, B и H — матрицы. Погрешность и шум V и W тоже векторы. Оказывается, что при этом, все уравнения будут работать точно так же, и алгоритм фильтра Калмана легко обобщается на многомерные величины.

А теперь, по традиции, пример на R

Фильтр Калмана можно найти в нескольких модулях R, для примера я буду использовать реализацию из модуля dlm. В данном примере мы применим фильтр Калмана к значениям индекса S&P500.

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

Для сравнения, на график, зеленым цветом, было добавлено 10-дневное экспоненциальное скользящее среднее Получилась картинка:

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

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

Применений множество. Представьте датчик топлива в баке автомобиля. Он же болтается там внутри, если просто записать его показания, а потом попытаться проанализировать, получится, что в какой-то момент в баке было 30 л, через секунду 50, а потом 27. А это просто волны внутри гуляют. Сколько же в этот момент было в баке? ФК может дать более вероятный ответ.

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

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

Если очень кратко, то суть в том, что большинство явлений подчиняются Гауссовскому распределению вероятностей.


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

Наиболее вероятный x посредине — это μ, математическое ожидание. Ширина этой "шляпы" зависит от σ 2 — это дисперсия, квадрат стандартного отклонения. Чем больше отклонение, тем шляпа шире и неопределенность больше.

У этой кривой такое уравнение:

p = 1 σ 2 π e — x — μ 2 2 σ 2 (1)

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

Читайте также:  Почему принтер печатает не теми цветами

Что же делает фильтр? Он помнит только одно значение x1 и его дисперсию σ 2 . Когда поступает новое значение x2 со своим отклонением r 2 , он умножает их (по правилам распределения, см. формулу 2) друг на друга и в результате будет новое значение x3 с погрешностью меньшей, чем у двух предыдущих!

Например, текущее значение фильтра — синее распределение. Поступает новое значение, со своим отклонением (красное). Фильтр перемножает их по правилам распределения (см. формулу 2) и получает новое распределение (зеленое). Оно гораздо точнее двух изначальных, т.к. его дисперсия меньше (см. формулу 3) и становится текущим значением фильтра.

Умножение распределений делается так:

x 3 = r 2 x 1 + σ 2 x 2 r 2 + σ 2 (2)

Дисперсия тоже уточняется:

σ 2 ’ = 1 1 r 2 + 1 σ 2 (3)

И тоже становится текущей. Поступает еще значение, состояние еще больше уточняется и так далее.

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

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

Матричная версия, классический фильтр Калмана:

# Multi-dimensional Kalman Filter from online course
# https://www.udacity.com/course/viewer#!/c-cs373/l-48723604/m-48670988
# written by Pavel Malinnikov, May 2013

from math import *
from random import *
import matplotlib. pyplot as plt

# implements basic operations of a matrix class

def __init__ ( self , value ) :
self . value = value
self . dimx = len ( value )
self . dimy = len ( value [ 0 ] )
if value == [ [ ] ] :
self . dimx = 0

def zero ( self , dimx , dimy ) :
# check if valid dimensions
if dimx 1 or dimy 1 :
raise ValueError , "Invalid size of matrix"
else :
self . dimx = dimx
self . dimy = dimy
self . value = [ [ 0 for row in range ( dimy ) ] for col in range ( dimx ) ]

def identity ( self , dim ) :
# check if valid dimension
if dim 1 :
raise ValueError , "Invalid size of matrix"
else :
self . dimx = dim
self . dimy = dim
self . value = [ [ 0 for row in range ( dim ) ] for col in range ( dim ) ]
for i in range ( dim ) :
self . value [ i ] [ i ] = 1

def show ( self ) :
for i in range ( self . dimx ) :
print self . value [ i ]
print ‘ ‘

def __add__ ( self , other ) :
# check if correct dimensions
if self . dimx != other. dimx or self . dimy != other. dimy :
raise ValueError , "Matrices must be of equal dimensions to add"
else :
# add if correct dimensions
res = matrix ( [ [ ] ] )
res. zero ( self . dimx , self . dimy )
for i in range ( self . dimx ) :
for j in range ( self . dimy ) :
res. value [ i ] [ j ] = self . value [ i ] [ j ] + other. value [ i ] [ j ]
return res

def __sub__ ( self , other ) :
# check if correct dimensions
if self . dimx != other. dimx or self . dimy != other. dimy :
raise ValueError , "Matrices must be of equal dimensions to subtract"
else :
# subtract if correct dimensions
res = matrix ( [ [ ] ] )
res. zero ( self . dimx , self . dimy )
for i in range ( self . dimx ) :
for j in range ( self . dimy ) :
res. value [ i ] [ j ] = self . value [ i ] [ j ] — other. value [ i ] [ j ]
return res

def __mul__ ( self , other ) :
# check if correct dimensions
if self . dimy != other. dimx :
raise ValueError , "Matrices must be m*n and n*p to multiply"
else :
# subtract if correct dimensions
res = matrix ( [ [ ] ] )
res. zero ( self . dimx , other. dimy )
for i in range ( self . dimx ) :
for j in range ( other. dimy ) :
for k in range ( self . dimy ) :
res. value [ i ] [ j ] + = self . value [ i ] [ k ] * other. value [ k ] [ j ]
return res

def transpose ( self ) :
# compute transpose
res = matrix ( [ [ ] ] )
res. zero ( self . dimy , self . dimx )
for i in range ( self . dimx ) :
for j in range ( self . dimy ) :
res. value [ j ] [ i ] = self . value [ i ] [ j ]
return res

# Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions

def Cholesky ( self , ztol = 1.0e-5 ) :
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
res = matrix ( [ [ ] ] )
res. zero ( self . dimx , self . dimx )

for i in range ( self . dimx ) :
S = sum ( [ ( res. value [ k ] [ i ] ) ** 2 for k in range ( i ) ] )
d = self . value [ i ] [ i ] — S
if abs ( d ) ztol:
res. value [ i ] [ i ] = 0.0
else :
if d 0.0 :
raise ValueError , "Matrix not positive-definite"
res. value [ i ] [ i ] = sqrt ( d )
for j in range ( i + 1 , self . dimx ) :
S = sum ( [ res. value [ k ] [ i ] * res. value [ k ] [ j ] for k in range ( self . dimx ) ] )
if abs ( S ) ztol:
S = 0.0
res. value [ i ] [ j ] = ( self . value [ i ] [ j ] — S ) / res. value [ i ] [ i ]
return res

def CholeskyInverse ( self ) :
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
res = matrix ( [ [ ] ] )
res. zero ( self . dimx , self . dimx )

# Backward step for inverse.
for j in reversed ( range ( self . dimx ) ) :
tjj = self . value [ j ] [ j ]
S = sum ( [ self . value [ j ] [ k ] * res. value [ j ] [ k ] for k in range ( j + 1 , self . dimx ) ] )
res. value [ j ] [ j ] = 1.0 / tjj ** 2 — S / tjj
for i in reversed ( range ( j ) ) :
res. value [ j ] [ i ] = res. value [ i ] [ j ] = — sum ( [ self . value [ i ] [ k ] * res. value [ k ] [ j ] for k in range ( i + 1 , self . dimx ) ] ) / self . value [ i ] [ i ]
return res

def inverse ( self ) :
aux = self . Cholesky ( )
res = aux. CholeskyInverse ( )
return res

def __repr__ ( self ) :
return repr ( self . value )

def kalman ( x , P ) :
for n in range ( len ( positions ) ) :

# measurement update
z = matrix ( [ [ positions [ n ] ] ] )
y = z — H * x
S = H * P * H. transpose ( ) + R
K = P * H. transpose ( ) * S. inverse ( )
x = x + ( K * y )
P = ( I — K * H ) * P

# prediction
x = F * x + u
P = F * P * F. transpose ( )

result. append ( x. value [ 0 ] [ 0 ] )
speed. append ( x. value [ 1 ] [ 0 ] )

data = [ 5 . + x * .3 for x in range ( 50 ) ]

positions = [ x + random ( ) * randint ( — 1 , 1 ) * 5 for x in data ]

x = matrix ( [ [ 0 . ] , [ 0 . ] ] ) # initial state (location and velocity)
P = matrix ( [ [ 1000 . , 0 . ] , [ 0 . , 1000 . ] ] ) # initial uncertainty
u = matrix ( [ [ 0 . ] , [ 0 . ] ] ) # external motion
F = matrix ( [ [ 1 . , 1 . ] , [ 0 , 1 . ] ] ) # next state function
H = matrix ( [ [ 1 . , 0 . ] ] ) # measurement function
R = matrix ( [ [ 4 . ] ] ) # measurement uncertainty
I = matrix ( [ [ 1 . , 0 . ] , [ 0 . , 1 . ] ] ) # identity matrix

plt. plot ( data , ‘b’ )
plt. plot ( positions , ‘r’ )
plt. plot ( result , ‘g’ )
plt. plot ( speed , ‘y’ )

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

# One-dimensional Kalman Filter testing for geo-track data smoothing
# Tracks were taken from http://www.openstreetmap.org/traces
# Pavel Malinnikov, May 2013

import matplotlib. pyplot as plt
import xml . etree . ElementTree as ET

tree = ET. parse ( ‘/Users/pavelmalinnikov/Downloads/1464857.gpx’ )
root = tree. getroot ( )

if len ( points ) == 0 :
print ‘Track loading failed’
exit ( )

def update ( mean1 , var1 , mean2 , var2 ) :
new_mean = ( var2 * mean1 + var1 * mean2 ) / ( var1 + var2 )
new_var = 1 / ( 1 / var1 + 1 / var2 )
return [ new_mean , new_var ]

def predict ( mean1 , var1 , mean2 , var2 ) :
new_mean = mean1 + mean2
new_var = var1 + var2
return [ new_mean , new_var ]

positions = [ [ float ( p. get ( ‘lon’ ) ) , float ( p. get ( ‘lat’ ) ) ] for p in points ]

longitudes = [ position [ 0 ] for position in positions ]
latitudes = [ position [ 1 ] for position in positions ]

motion_sig = 1 .
measurement_sig = 10 .

lng = longitudes [ 0 ]
lat = latitudes [ 0 ]
sigLng = 1 .
sigLat = 1 .

for i in range ( len ( positions ) ) :
lng , sigLng = update ( lng , sigLng , longitudes [ i ] , measurement_sig )
lng , sigLng = predict ( lng , sigLng , 0 , motion_sig )
resultLng. append ( lng )

lat , sigLat = update ( lat , sigLat , latitudes [ i ] , measurement_sig )
lat , sigLat = predict ( lat , sigLat , 0 , motion_sig )
resultLat. append ( lat )

kalman = [ [ resultLng [ i ] , resultLat [ i ] ] for i in range ( len ( positions ) ) ]

plt. plot ( * zip ( *positions ) , color = ‘r’ )
plt. plot ( * zip ( *kalman ) , color = ‘g’ )

Читайте также:  Программа кристалл для проверки жесткого диска

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

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

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

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

Carc 29.05.2013 09:42

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

Эрбол 05.09.2013 07:05

Отличная статья. Автору почет и уважение

Сергей 03.10.2013 08:58

Замечательная статья! Я её использую на занятиях со студентами (специальность не техническая, будущие менеджеры). Но надо немного поправить текст в этом месте "Например, текущее значение фильтра — синее распределение. Поступает новое значение, со своим отклонением (красное). Фильтр перемножает их и получает новое распределение (зеленое)". После ". перемножает их" надо добавить и умножает на нормировочную константу. Если это не сделать, зеленое распределение будет ниже синего и красного, соответственно площадь под кривой меньше единицы — нарушается условие нормировки. Эти три красивые кривые наглядно демонстрируют, что фильтр Калмана реализует байесовский подход. Я всегда объясняю формулу Байеса так — апостериорное распределение равно произведению трёх сомножителей: 1-нормировочная константа, 2-априорное (в реализации Калмана эктраполированное) распределение, 3- функция правдоподобия. С уважением к автору Павлу Малинникову, спасибо за статью!

Павел Малинников

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

Дмитрий 28.11.2013 12:25

" Что же делает фильтр? Он помнит только одно значение x1 и его дисперсию σ2. Когда поступает новое значение x2 со своим отклонением r2,"

r2 — r в квадрате ето же дисперсия. или я ошибаюсь?

Павел Малинников

Не ошибаетесь, все правильно. Везде используется дисперсия. ". поступает новое значение x2 со своим отклонением r (в квадрате)". Квадрат отклонения — это дисперсия и есть.

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

nagim 01.12.2013 14:50

Добрый день,
искал вчера реализацию калмановского фильтра на питоне, взял вашу, примерил — работает. Теперь хочу разобраться почему 🙂
Правильно ли я понимаю, что в случае фильтрации функции y(x), на вход широты нужно подавать x, а на вход долготы — y? При фильтрации лишь одной переменной результаты много хуже (кстати, подскажите, пожалуйста, почему выгоднее фильтровать именно пару значений [x;y]?). С реализацией по скользящему среднему, конечно, не идет ни в какое сравнение — калман держит сглаженной как саму функцию, так и ее производную.

Павел Малинников

По иксу — долгота, широта — по игреку.

На питоне я просто проверял идею. Для промышленного применения я потом сделал матричную реализацию на Objective-C и там я фильтрую не только пару координат X, Y, но и вектор скорости по X, Y, так что в матричных вычисления участвует матрица 4*1, остальные матрицы 4*4.

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

Алекс 12.12.2013 08:31

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

Павел Малинников

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

Александр Новинский 14.06.2014 17:11

Добрый день, спасибо за статью.
Одного не могу понять.
Когда приходит новое значение с показания датчика, откуда узнать какого его отклонение (т.е. дисперсия) ? Я могу только предположить, что эти параметры задаются самостоятельно на основе технических характеристик датчиков. Т.е., например, температура с точностью до одной десятой градуса. Значит ли это, что у такого датчика дисперсия равна 0.1? Или как, например, определить дисперсию показания вайфай локатора? Вряд ли там какая-то точность указана — слишком уж все зависит от взаимного располодения точек доступа вайфай да и силы сигнала.

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

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

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

(1)

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

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

(2)

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

E<> =

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

E<> =

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

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

(3)

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

= + , (4)

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

Читайте также:  Как восстановить пароль на айфоне через айтюнс

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Заключение

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

Рецензенты:

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

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

Ссылка на основную публикацию
Adblock detector