Анализ эффективности расширенного фильтра Калмана, сигма-точечного фильтра Калмана и сигма-точечного фильтра частиц Текст научной статьи по специальности «Компьютерные и информационные науки»

Vol. 19, № 02, 2016

Civil Avition High TECHNOLOGIES

УДК 519.24

АНАЛИЗ ЭФФЕКТИВНОСТИ РАСШИРЕННОГО ФИЛЬТРА КАЛМАНА, СИГМА-ТОЧЕЧНОГО ФИЛЬТРА КАЛМАНА И СИГМА-ТОЧЕЧНОГО ФИЛЬТРА ЧАСТИЦ

И.А. КУДРЯВЦЕВА

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

Ключевые слова: нелинейная фильтрация, расширенный фильтр Калмана, ИшсеШЫ-преобразование, сигма-точечный фильтр Калмана, сигма-точечный фильтр частиц.

ВВЕДЕНИЕ

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

При линейной модели и аддитивных гауссовских шумах оптимальное решение задачи получают с использованием фильтра Калмана (ФК). В случае если модель объекта наблюдения или модель измерений нелинейные, выбор алгоритма зависит от характера случайных воздействий. Если случайные воздействия подчинены гауссовскому закону распределения, то для данного случая применим расширенный фильтр Калмана (РФК, ЕКБ) [1, 2, 6] и сигма-точечный фильтр Калмана (ИКБ) [3-5]. Если случайные воздействия могут быть негауссовскими, то используются фильтры частиц [2, 7].

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

1. ПОСТАНОВКА ЗАДАЧИ

Случайный процесс X() в моменты времени t = t0,tк,... описывается следующим разностным уравнением:

Хк+1 = I(, V), к = 0,1,2,..., X(о) = Хо, (1)

где Хк = X (к )е Мп - вектор состояния системы, Х0 ~ р (х) - начальное состояние системы, I (х, V): Ж"х" ^ Мп - заданная вектор-функция, Ук е Мп - дискретный

векторный белый шум с нулевым математическим ожиданием и ковариационной матрицей Xv.

Случайный процесс Х () в моменты времени t = tk,... доступен косвенным

наблюдениям, удовлетворяющим соотношению

Civil Avition High TECHNOLOGIES

Vol. 19, № 02, 2016

Yk+1 = g((+l,Wk+1), k - 0,1,2,...,

(2)

где Ук е Ш.т - вектор измерений, g (х, : Михт ^ Мт - заданная вектор-функция, Шк е №т -

дискретный векторный белый шум с нулевым математическим ожиданием и ковариационной матрицей Е^.

Требуется в каждый момент времени I = tк,... по результатам всех имеющихся к этому моменту наблюдений Уу = {У[,...,у} получить вектор оценки Хк состояния процесса Хк

с использованием расширенного фильтра Калмана (ЕКБ), сигма-точечного фильтра Калмана (иКБ), сигма-точечного фильтра частиц (ЦРБ).

2. АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ С ИСПОЛЬЗОВАНИЕМ РАСШИРЕННОГО ФИЛЬТРА КАЛМАНА

В расширенном фильтре Калмана (РФК, ЕКБ) вводится предположение о гауссовском распределении вектора состояния, который подвергается линейному преобразованию. Данное линейное преобразование представляет собой линеаризованные функциональные зависимости в уравнении состояния (1) и в уравнении измерений (2), что дает возможность применения алгоритма фильтра Калмана (ФК).

Пусть математическая модель нелинейной дискретной стохастической системы имеет вид

Хк+1 = /(ХкЛ), V ~ (0,Еу), к = 0,1,2,..., Ук+1 = g(к+Л+1), Шк ~ (0,Е№), к = 0,1Д...

Применяя разложение уравнения состояния в ряд Тейлора в окрестности текущей оценки Хк и отбрасывая члены второго и более высоких порядков, получаем

Xk+1 = f (( ,o)+f

X,

(Xk - Xk )+f

V = AkXk + Uk + Vk,

(3)

„ df где Ak =f.

dx

B -df , Bk "äV

, ик = (/ (Хк, 0) - АкХк) - известная вектор-функция, Ук - дискретный

Х^к

векторный белый шум с нулевым математическим ожиданием и ковариационной матрицей Вк ЕуВТк .

Выполняя аналогичные преобразования над уравнением измерений в окрестности прогноза X, , имеем

Ук+1 = g ((,o)+|

X,

((- Xk )+dg

Zk+1

(4)

X

n dg

где Ck+1 = — dx

D

Uh-лЛ — _

n^k+1

X

dw

, Zk+1

X

■ (g ((,0)- Ск+1 Хк) - известная вектор-функция, Шк -

дискретный векторный белый шум с нулевым математическим ожиданием и ковариационной матрицей Вк+1Е.

Vol. 19, № 02, 2016

Civil Avition High TECHNOLOGIES

Уравнения состояния (3) и наблюдения (4) являются линейными, что позволяет применить алгоритм рекуррентной фильтрации Калмана, блок-схема которого представлена на рис. 1, для получения оценки вектора состояния [6, 8].

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

Рис. 1. Блок-схема алгоритма расширенного фильтра Калмана

3. АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ С ИСПОЛЬЗОВАНИЕМ СИГМА-ТОЧЕЧНОГО ФИЛЬТРА КАЛМАНА

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

В основе алгоритма сигма-точечного ФК лежит ЦпвсеЩеё-преобразование, основная идея которого заключается в том, что аппроксимации подвергается не произвольная нелинейная функция, а распределение случайной величины. К примеру, пусть имеется некоторый случайный вектор X, который подвергается нелинейному преобразованию У = g (X). Предполагается, что математическое ожидание тх и ковариационная матрица Ях данного вектора X

Civil Avition High TECHNOLOGIES

Vol. 19, № 02, 2016

известны. Далее вводится по определенному правилу набор векторов X1, 1 = 1,..,К, называемых сигма-точками, таких, что их среднее и ковариационная матрица равны тх и ЯХ . Каждой

частице ставится в соответствие некоторой вес w1, 1 = 1,..,К, таким образом, чтобы обеспечить несмещенную оценку математического ожидания введенного набора частиц, причем:

К

I w1 = 1.

1=1

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

m

X

K K

=Xwx' , Rx = I w ((' - mix )(' - mhx) .

i=1 i=1

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

В качестве соотношений, применяемых для нахождения сигма-точек, в данной работе рассматривались следующие [4, 5, 8]:

X0 = mx, w0 = w0,

Хг = mx +.

K

'1 -

(R

x'+K =m

K

x

1 - w 0

(R)

>2

w =■

W =•

1 - w0 2K 1 - wc ~2K

i = 1,..., K,

где w - параметр, значения которого отвечают за положения сигма-точек относительно математического ожидания, (Ях - 1 -й столбец или строка матрицы (Ях ^ .

Комбинируя процедуру получения оценок на основе ишсеЩеё-преобразования с алгоритмом фильтрации Калмана, получают алгоритм сигма-точечного ФК, представленный на рис. 2 [3]. Для применения алгоритма сигма-точечного ФК к системам (1)-(2) необходимо ввести

новый вектор состояния X" = (Хк, ¥к, Жк) . Тогда вектор оценки нового вектора состояния Х01 и оценка его ковариационной матрицы в начальный момент времени выбираются рав-

V а

ными x0

f x ^ x

0

0

У

и tf

Pn

0 0

0 0

Е.

соответственно [4]. Построение набора сигма-точек

V

осуществляется применительно к новому введенному вектору состояния.

4. АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ С ИСПОЛЬЗОВАНИЕМ СИГМА-ТОЧЕЧНОГО ФИЛЬТРА ЧАСТИЦ

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

Vol. 19, № 02, 2016

Civil Avition High TECHNOLOGIES

В силу того, что характер р(хк\Ук ) не известен, сгенерировать случайные точки, называемые частицами, затруднительно. Вводится распределение значимости хк\Ук ) , в большинстве случаев выбираемое гауссовским, на основании которого происходит генерация частиц. Тогда, для произвольного нелинейного преобразования случайного вектора Хк, имеющего распределение

р(хк\Ук) , справедливо следующее:

M

h(Xk )Yk] = {h(xk )p(xk\Y,k)dxk = Jh(xk q(xk\Yk)dxk.

■ q(xk Yi)

Далее, на основании полученного из распределения q( хк\Ук) набора частиц Х'к,/ = 1,..,Ыр, опираясь на метод Монте-Карло и используя правило Байеса, следует:

n„

h (Xk ) = Z \h (Xk),

i=1

p( xkYik) = cp (kX )p (|Yk-1)

q(xk\Yik) q(xk\Yk)

где с - нормирующий множитель; - веса частиц; р (хк|У1к 1) определяется уравнением состояния (1); р(ук\Хк) - уравнением измерений (2); q(xk|У1k) - распределение значимости.

Рис. 2. Блок-схема алгоритма сигма-точечного фильтра Калмана

Civil Avition High TECHNOLOGIES

Vol. 19, № 02, 2016

4. РЕЗУЛЬТАТЫ МОДЕЛИРОВАНИЯ

На основании изложенных алгоритмов фильтрации разработан пакет программ с использованием среды разработки MATLAB и Microsoft Visual Studio C#. Применение Microsoft Visual Studio C# позволило создать приложение, имеющее графический интерфейс для визуализации входных и выходных параметров. Для этого в MATLAB с помощью программного пакета Deployment Tool были созданы dll-библиотеки, которые интегрировались в C#. При помощи созданного программного обеспечения проведены серии вычислительных расчетов для различных модельных примеров. Ниже приведены результаты численного решения для следующих моделей вида (1)-(2) [10]:

а) Xk+1 + 0.7-Vk, Yk+i = 0.6Xk+i + 0.2-Wk+u k = 0,1,2,...;

1 + Xk

б) Xk+1 = TX- + cos(k-Xk) + 0.7Vk, Yk+1 = 0.6Xk+1 + 0.2-Wk+U к = 0,1,2,...;

1 + Xk

в) Xk+1 =-X-2 + 0.7-Vk, Yk+1 = Xk+1 + Xk+1 + 0.2-Wk+1, k = 0,1,2,...,

1+Xk

где X0 ~ N(mx,Px) - начальное состояние системы; Vk, Wk - дискретные гауссовские белые шумы с нулевыми математическими ожиданиями и ковариационными матрицами Xv и соответственно.

О 10 20 »

60 «t ТО M M 1«

Рис. 3. Оценка Хк, полученная на основании алгоритмов РФК, сигма-точечного ФК, сигма-точечного фильтра частиц для следующих значений параметров: а, г - mx = 1.5, Р = 1 = 3, ^ = 1;

б - m = 0, P = 1, X = 1, X = 1; в - m = 6, P = 1, X = 3, X = 1

Vol. 19, № 02, 2016

Civil Avition High TECHNOLOGIES

При наличии нелинейной аддитивной модели с гауссовскими случайными воздействиями результаты, полученные на основании РФК, сигма-точечного ФК и сигма-точечного фильтра частиц близки, что наглядно видно на рис. 3а. Для оценки эффективности результатов фильтрации наряду с графической интерпретацией использовалось значение среднеквад-ратической ошибки (тЕКР, тикр, тиРЕ). Для случая, проиллюстрированного на рис. 3а,

тЕКР = 0.1532, тиКР = 0.1405, тирр = 0.1661. При появлении в уравнении нелинейностей, поведение которых в окрестности опорной траектории, существенно отличается от линейной зависимости, РФК дает оценку хуже, в сравнении с сигма-точечным ФК и сигма-точечным фильтром частиц, которые непосредственно работают с заданными нелинейностями.

Рис. 4. Блок-схема алгоритма сигма-точечного фильтра частиц

Данный факт прослеживается на примере значений среднеквадратической ошибки: тЕКР = 4.5926, тК = 0.4212, тиРЕ = 0.5455 (рис. 36) и тЕКР = 2.1101, тК = 0.0511,

тиРЕ = 0.0485 (рис. 3в). Кроме того, при работе с сигма-точечным фильтром частиц, используя

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

Civil Avition High TECHNOLOGIES

Vol. 19, № 02, 2016

СПИСОК ЛИТЕРАТУРЫ

1. Пантелеев А.В., Руденко Е.А., Бортаковский А.С. Нелинейные системы управления: описание, анализ и синтез. - М.: Вузовская книга, 2008.

2. Chen Z. Bayesian filtering: From Kalman filters to particle filters, and beyond // Statistics.

2003. № 1. Pp. 1-69.

3. Haug A. A tutorial on Bayesian estimation and tracking techniques applicable to nonlinear and non-Gaussian process / MITRE Technical Report MTR 05W0000004. MCLean: The MITRE Corporation, 2005.

4. Julier S.J., Uhlmann J.K. Unscented filtering and nonlinear estimation // Proc. Of IEEE.

2004. № 3. Pp. 401-422.

5. Julier S., Uhlmann J., Durrant-Whyte H. A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators // IEEE Trans. On Aut. Contr. Vol. 45, No. 3. 2000.

6. Миллер Б.М., Панков A.P. Теория случайных процессов в примерах и задачах. - М.: Физматлит, 2002. - 320 с.

7. Doucet A., Johansen A.M. A tutorial on particle filtering and smoothing: Fifteen years later // Oxford handbook of nonlinear filtering. Oxford: Oxford University Press, 2011. Pp. 656-704.

8. Simon D. Optimal state estimation: Kalman, H ^ and nonlinear approaches. John Wiley & Sons, 2006.

9. Коновалов A.A. Основы траекторной обработки радиолокационной информации: в 2 ч. - Спб.: Изд-во СпбГЭТУ «ЛЭТИ», 2014. Ч. 2. - 180 с.

10. Кудрявцева И.А., Насонова Л.Р. Применение фильтров частиц для решения задачи оценивания состояния дискретный стохастических систем // Авиация и космонавтика - 2015. XIV Международная конференция, Москва, 16-20 ноября 2015 г.: Тез. докл. - М.: Типография «Люксор», 2015. - С. 437-438.

EFFICIENCY ANALYSIS OF EXTENDED KALMAN FILTERING, UNSCENTED KALMAN FILTERING AND UNSCENTED PARTICLE FILTERING

Kudryavtseva I.A.

The paper contains algorithms for solving the problem of nonlinear filtering. The nonlinear approximate filters presented are: the extended Kalman filter (EKF), the uncented Kalman filter (UKF) and unscented Particle Filter (UPF). The flow-charts for the listed algorithms are presented and a corresponding software complex based on these algorithms is developed. Using this complex a number of worked-out numerical simulations for both linear and nonlinear models are resulted.

Key words: nonlinear filtering, Extended Kalman filter, Unscented Kalman filter, Unscented Particle filter, Sequential Monte-Carlo estimation.

REFERENCES

1. Panteleev A.V., Rudenko E.A., Bortakovsky A.S. Nonlinear control systems: description, analysis and synthesis. Moscow. 2008. (in Russian).

2. Chen Z. Bayesian filtering: From Kalman filters to particle filters, and beyond // Statistics. 2003. № 1. Pp. 1-69.

3. Haug A. A tutorial on Bayesian estimation and tracking techniques applicable to nonlinear and non-Gaussian process. MITRE Technical Report MTR 05W0000004. MCLean: The MITRE Corporation, 2005.

Vol. 19, № 02, 2016

Civil Avition High TECHNOLOGIES

4. Julier S.J., Uhlmann J.K. Unscented filtering and nonlinear estimation. Proc. Of IEEE. 2004. No. 3. Pp. 401-422.

5. Julier S., Uhlmann J., Durrant-Whyte H. A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators. IEEE Trans. on Aut. Contr. Vol. 45, No. 3, 2000.

6. Miller B.M., Pankov A.R. Stochastic processes: theory and practice. Moscow. 2002. 320 p. (in Russian).

7. Doucet A., Johansen A.M. A tutorial on particle filtering and smoothing: Fifteen years later // Oxford handbook of nonlinear filtering. Oxford: Oxford University Press, 2011. Pp. 656-704.

8. Simon D. Optimal state estimation: Kalman, H ^ and nonlinear approaches. John Wiley & Sons. 2006.

9. Konovalov A.A. Basic principles of radar data processing. Saint-Petersburg. 2014. 180 p. (in Russian).

10. Kudryavtseva I.A., Nasonova L.R. Solution of stochastic discrete filtering problem by using particle filters. Abstracts of 14th International Conference "Aviation and Cosmonautics". Moscow, 2015, Pp. 437-438. (in Russian).

СВЕДЕНИЯ ОБ АВТОРЕ

Кудрявцева Ирина Анатольевна, 1982 г.р., окончила МАИ (2005). Кандидат физико-математических наук, доцент кафедры «Математическая кибернетика» Московского авиационного института, автор более 30 научных публикаций. Область научных интересов - математические методы моделирования, методы Монте-Карло, электронный адрес: irina.home.mail@mail.ru.