Применение ud-модификации сигма-точечного фильтра Калмана и сигма-точечного фильтра частиц к решению задач трассового анализа Текст научной статьи по специальности «Математика»

Ovil Aviation High Technologies

Vol. 21, No. 02, 2018

ИНФОРМАТИКА, ВЫЧИСЛИТЕЛЬНАЯ ТЕХНИКА И УПРАВЛЕНИЕ УДК 519.24

DOI: 10.26467/2079-0619-2018-21-2-8-21

ПРИМЕНЕНИЕ UD-МОДИФИКАЦИИ СИГМА-ТОЧЕЧНОГО ФИЛЬТРА КАЛМАНА И СИГМА-ТОЧЕЧНОГО ФИЛЬТРА ЧАСТИЦ К РЕШЕНИЮ ЗАДАЧ ТРАССОВОГО АНАЛИЗА

И.А. КУДРЯВЦЕВА1, М.В. ЛЕБЕДЕВ1

1 Московский авиационный институт (национальный исследовательский университет),

г. Москва, Россия

Работа выполнена при поддержке РФФИ, грант № 17-08-00530 А

В работе рассматривается решение задачи оценивания вектора состояния дискретной стохастической системы по имеющимся наблюдениям с использованием модификации сигма-точечного фильтра Калмана (Unscented Kalman Filter, UKF) и сигма-точечного фильтра частиц (Unscented Particle Filter, UPF), построенных на основе UD-разложений ковариационных матриц, при этом используется скаляризованная форма записи уравнений фильтра Калмана. Идея метода частиц заключается в использовании набора случайных точек (частиц) с ассоциированными весами, аппроксимирующих апостериорную плотность вероятности. В силу того, что апостериорное распределение неизвестно, для генерации частиц выбирается иное распределение - распределение значимости. Алгоритм сигма-точечного фильтра частиц является разновидностью фильтров частиц со встроенной процедурой получения параметров распределения значимости, которое полагается гауссовским, на основе Unscented-преобразования. С помощью разработанных алгоритмов проведено численное решение задачи трассового анализа для двух случаев. В первом случае рассматривается задача определения координат подвижного объекта только по зашумленным наблюдаемым значениям его пеленга (задача пассивной локации). Во втором случае рассматривается задача активной локации, когда наблюдателю доступны, помимо пеленга, зашумленные значения дистанции до сопровождаемого объекта. Кроме того, в модель движения в задаче активной локации добавлен дополнительно маневр, как угол направления вектора скорости. При численном моделировании для случая активной локации в качестве наблюдений выступал произвольный маневр, отличный от заданного в модели движения с целью проверки робастности рассматриваемых алгоритмов к изменению модели движения для наблюдаемого объекта.

Ключевые слова: нелинейная фильтрация, методы Монте-Карло, UD-преобразование, Unscented-преобразование, сигма-точечный фильтр Калмана, сигма-точечный фильтр частиц.

ВВЕДЕНИЕ

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

В работе рассматривается решение задачи сопровождения цели для пассивного и активного режима с помощью разработанных модификации алгоритмов сигма-точечного фильтра Калмана и сигма-точечного фильтра частиц на основе UD-разложений матриц ковариации. Для решения ра^матриваемой задачи существуют иные подходы [3-5], в частности метод псевдоизмерений [3].

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

Пусть состояния динамической системы Xk, к = 0,1,2,... подчиняются следующему разностному уравнению:

Xk+1 = f (Xk, Vk), X0 ~p(x), к = 0,1,2,..., (1)

где Xk e Mn - вектор состояния системы, X0 - начальное состояние системы с известным законом распределения p (x), f (x, v) : M2n ^ Mn - известная вектор-функция, Vk e M" - дискретный векторный белый шум с математическим ожиданием mv и ковариационной матрицей Ev .

Случайный процесс Xk, k = 0,1,2,... доступен косвенным наблюдениям, удовлетворяющим уравнению

Yk+, = g(Xk+1,^k+1), k = 0,1,2,..., (2)

где Yk e Mq - вектор измерений, g (x, w): Mn+q ^ Mq - известная вектор-функция, Wk e Mq -дискретный векторный белый шум с математическим ожиданием mw и ковариационной матрицей ^w.

Требуется получить вектор оценки Xk состояния процесса Xk по результатам наблюдений Y1 ={Y(,..., Yk} c использованием модификаций алгоритмов сигма-точечного фильтра Кал-мана и сигма-точечного фильтра частиц на основе UD-разложения матриц ковариаций.

UD-МОДИФИКАЦИЯ СИГМА-ТОЧЕЧНОГО ФИЛЬТРА КАЛМАНА

Сигма-точечный фильтр Калмана, в основе алгоритма которого лежит Unscented-преобразование, впервые был предложен в работах [1] как альтернатива классической реализации с целью преодоления ограничений, преимущественно связанных с применением процедуры фильтрации Калмана к нелинейным моделям путем линеаризации, которая проводится посредством вычисления матрицы Якоби.

Unscented-преобразование

N T

Вводится расширенный вектор состояния системы X^eM X а : X^=(Xk,Vk, Wk) , NX<1 = 2n + q, где n - размерность вектора состояния системы, q - размерность вектора наблюдения. Далее на каждом этапе по времени согласно определенному правилу строится набор сигма-точек '', i = 0,...,L -1, L = 2NXa +1, локализующихся в окрестности полученной

на предыдущем этапе оценки Xk вектора состояния системы. Каждой сигма-точке ставится в соответствие пара (wm,i, wc,i), i = 0,...,L -1, элементами которой являются веса точек. При этом

L-1 L-1

предполагается, что ^ wm,i = 1, ^ wc,i = 1. Веса задаются таким образом, чтобы обеспечить не-

i=0 i=0

Ovil Aviation High Technologies

Vol. 21, No. 02, 2018

смещенную оценку математического ожидания и ковариационном матрицы введенного набора точек:

wmfi = ■

X

Nxa+Ä

wm =

1

2 (a+XY

c, 0

w =-

X

Nxa+X

+1 -Г + ß wci =

1

i = 1.....L-1.

2 (fc^)'

Для получения набора сигма-точек использовались соотношения [1, 6, 10]

Хк'° = XI, хг = 4а)г,

а,г+N

Xk

а = ха-

, 1 = 4 Nx a+X , Х = Г2 (Nxa+ p)-Nx a , i = 1,..., N^ ,

где p - заданные постоянные параметры.

Далее каждая сигма-точка в отдельности пропускается через уравнение состояния (1) Xk = f (Xk"'), X'k е Мn, i = 0,...,L-1 с целью получения прогнозного значения оценки вектора

^ l-1 . _. —

состояния Xk = ^ wm'' Xk и матрицы ковариации ошибки прогноза Pk.

i=0

Среди способов увеличения вычислительной эффективности фильтра Калмана [7] можно выделить группу квадратно-корневых алгоритмов, в частности UD-реализацию фильтра Калмана, впервые предложенную в работе Бирмана (G.J. Bierman) [8]. В рамках данной реализации на этапе предсказания и этапе коррекции осуществляется UD-разложение матрицы ковариации ошибки предсказания и матрицы ковариации ошибки оценки фильтра соответственно.

Идея UD-разложения состоит в представлении матрицы P е Мnxn в виде P = UDUT, где

Г1

U е Кnxn, D е Кnxn U =

12

u

1n

u

0 1 0 0 0 1

2n

D =

d11 0

0 d

22

0 0

v 0 0 ... dnnj

Такое разложение всегда существует для симметрической положительно-определенной квадратной матрицы [9]. Для поиска матриц и,В применялся алгоритм из [8].

1Ш-разложение матрицы ковариации ошибки предсказания

Для нахождения оценки матрицы ковариации ошибки предсказания по полученному множеству обновленных значений сигма-точек Хк = /(Xк')> ' = 0,...,Ь— 1 используются следующие соотношения:

_ =( X' — с\ X' — X Г

Pk =

{xk - Xk )wc { xk - Хк [ , Wc = diag (wc,i ),i = 0,...,

c,0

w =■

X

Nxa +X

+1+ ß; wc =■

1

(a +X

L-1, , i = 1,...,L-1,

где I Xk - Xk je MnxL , Wc e MLxL - матрица весовых коэффициентов, соответствующих сигма-

точкам xk, i = 0,...,L-1.

Используя ортогонализацию Грамма - Шмидта [9], можно найти Uk e Mnxn ,Vk e MnxL :

|Xk - Xk j = UV . При этом согласно [2] может быть применен следующий алгоритм:

_ _ _ « а$ШсуТ _ а$ШСгТ _ 1

Уп _ ап' _ а? ^ С Т Уг' г _ С т ' $'г _ 1'"-'П'

п п $ $ г уЛСуТ г ^СуТ

где а$ _ Хк $ ~XXк - $-я строка матрицы |Хк - XXк|, * _ 0,•••' Ь-1; - $-я строка матрицы V ; и$г - элемент матрицы ик, стоящий на пересечении $-й строки матрицы и г-го столбца.

Тогда Рк _ {Хк - Хк} шс {Хк - Хк } _ икГкЖсГктЩ _ илиг' 4 _ ^V/ . 1Ю-разложение матрицы ковариации ошибки оценки фильтра

Этап коррекции реализуется в скаляризованной форме, что позволяет избежать процедуры обращения матриц. Скаляризованные уравнения для получения матрицы ковариации ошибки оценки фильтра Калмана имеют вид [6]

(3) _ 1 Р (3) Р (3) _ Р 1 К (3)( К (3 )\т 3 _ 1 q

к ~ -(3) гху,к< гк+1 ~ к -(3) лк у^к } > V ~ 1>-">'/,

УУ,к уу,к

где К3 > — значения коэффициента усиления фильтра на 3-й итерации, РХу\ е Кпх1, РУу'к е — значения оценок взаимной ковариации и ковариации наблюдений на 3-й итерации, q - размерность вектора наблюдения. Тогда

K-k =

Pj = Pk - -j 4J} (k( })Г =UkVkWcVkTUTk - pj {UkVkWc lYkj - Yk,7 jT lYkj - Yk,7 jwcVkTUkT

PW V > p<

yy,k yy,k

= Uk

{ > p(J)K^ Iki-^k jj Hk,-'k , T ^T

VkWCVkT - PJ) Vkwc ll - j ll - Yk,j}WCVk7

V

yy,k

UT,

где {гкг 3 - 7к 3} е №1хЬ - 3-я строка матрицы {7к - 7к }, столбцы которой содержат центрированные значения преобразованных сигма-точек по уравнению наблюдений (2). Применяя алгоритм ЦО-разложения [7], получаем

0() _УкШсУкт --3Укшс {{ -{ -3%т _ий (и3 )т.

Руу'к

Civil Aviation High Technologies

Таким образом,

Vol. 21, No. 02, 2018

P(J) _ U TT(J) DJ) (TT(J) ) ÜT . TT(J) _ U TT(J) D(J) _ d(j) / - 1 q Pk+1 " UkUG,kDG,k\UG,k ) Uk' Uk+1~ UkUG,k' Dk+1~ uG,k' J -

Ниже приведен алгоритм сигма-точечного фильтра Калмана на основе UD-разложения матриц ковариаций.

АЛГОРИТМ СИГМА-ТОЧЕЧНОГО ФИЛЬТРА КАЛМАНА НА ОСНОВЕ UD-РАЗЛОЖЕНИЯ МАТРИЦ КОВАРИАЦИЙ

1. Инициализировать Х0 , Р0 , N, п, д, где N - количество итераций по времени, п -размерность вектора состояния системы, д - размерность вектора наблюдения.

2. Положить к = 0, Nv а = 2п + д, Ь = 2 ^ а +1.

X X

3. Получить ЦБ-разложение матрицы Рк : [Цк,Бк] = ЦБ(Рк),

4. Получить ЦБ-разложение матриц Еу,Е^: Ц,Д,Ш(Еу); Ц,] = Ш(Е^) .

5. Сформировать вектор системы X" = (Xк , тч, шм,) .

6. Сформировать ^ = .

7.

f Uk 0 0 > f Dk 0 0 1

U ? _ 0 Uv 0 Dk _ > Dk " 0 Dv 0

V 0 0 Uw J v 0 0 Dw j

8. Сгенерировать набор сигма-точек:

'0 _X?, X?_X?+у(S? ),

?,i+ N

Xk

? - X? -

у(а ) , у-4 NX? + 1, ((? + p)-NX?, i _ 1,...,

Nx? •

ния

9. Получить преобразованные значения сигма-точек на основании уравнения состоя: Хк = /(ХГ ),Хк е Мп, г = 0,Ь-1.

10. Найти прогнозное значение Хк :

L-1

Хк -X wm,Xt

k >

wm'° _•

1

i-0

NX? +1-

_

2 (nx ? +1)

, i _ 1,..., L -1.

11. Получить ЦБ-разложение матрицы Рк :[ик,Бк ] = ЦБ (Рк ) .

12. Сгенерировать набор сигма-точек:

s? _U?p?, Xkk'° _xk?, Xr _X? +у(s?)i, xr x? _xxk -у(s?)i,

у Nx? +1, 1 _ (( + p) - Nx?, i _ 1,..., Nx?.

13. Получить преобразованные значения сигма-точек на основании уравнения наблюдения: Yi = g((f ),i = 0,...,L-1.

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

4j = Z wm,'Yij, pmk = Z wc,i (Yk,j - Kj )(Yk,j - YlkJ), i=0 i=0

Ъ = Z w"(Xk - ^ j

Kk+1 = Pxy,k ((yy,k ) ,

z0+1) = z() + K (Y - y ) 7l = X

Zk+1 " Zk+1 + Kk+ЦYk+1, j Yk, j ), zk+1 " Xk,

G(j) = Vk Wc (Vk )T - Vk Wc II - Yk,j jT ((yy,k )-1 j - Yk,j T Wc (Vk )T, [Uj,Dj] = UD((j}), j = 1,...,q, i = 0,...,L-1.

15. Положить Uk+1 = UUGk, Dk+1 = DG,k, X^k+1 = Zf+1.

16. Если k = N, то вычисления завершить. В противном случае положить k = k +1 и перейти к п. 5.

UD-МОДИФИКАЦИЯ СИГМА-ТОЧЕЧНОГО ФИЛЬТРА ЧАСТИЦ

Для нахождения апостериорной плотности p( xk|Y1k) вводится набор случайных частиц (точек) X'k, i = 1,..., Np, таких, что

1 Np

p (К )« Z5(xk - Xk ) = p (xk\Y,k ), N p i=1

где 5 (•) - дельта-функция Дирака.

Тогда для произвольной нелинейной функции f (xk ) справедливо

1 N p

M(f (xk)) = Jf (xk)p(xk|Y1k- — Zf (Xk). (3)

Np i =1

В силу того, что характер p(xk|Y1k) не известен, сгенерировать случайные точки затруднительно. Вводится распределение значимости q(xk|Y1k), в большинстве случаев выбираемое

гауссовским, на основании которого происходит генерация частиц. Таким образом, (3) можно переписать в виде

Civil Aviation High Technologies

Vol. 21, No. 02, 2018

M

(f h )H f h ]Шq [xtY K =i f ^ qЫгП

dxk =

^ if (Xk Kq W? H = r p (x, )p (xk)

1 Np (

if (x, )wkq (У? )dxk NP ZW,f(Xk)

i

———-q (xk|yik )

q(xk\yi) v '

3 k

Nv k

W

Np

k WW If X ),

г=1

где ~м'к - веса частиц Х'к,г = 1,...,N , которые могут быть вычислены на основе следующих соотношений:

wk =■

(У) = p(у/"U)p(xkyk-1) ^ cp(yk\xk)p(xkYik-1)

(Yk) p (У-1 )q (|yk)

<xk|Yik )

Wu

(lXk)p(-1) t = j N q(x^) ...... p

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

АЛГОРИТМ СИГМА-ТОЧЕЧНОГО ФИЛЬТРА ЧАСТИЦ НА ОСНОВЕ UD-РАЗЛОЖЕНИЯ МАТРИЦ КОВАРИАЦИЙ

1. Инициализировать Х0, Р0, N, п,т, где N - количество итераций по времени, п - размерность вектора состояния системы, q - размерность вектора наблюдения.

2. Положить к = 0, Nxa = 2п + q, Ь = 2N „ +1.

X X

3. Получить ЦБ-разложение матриц №: [и„,] = Ш(Еу); [и„,] = Ш(Е№) .

4. Получить ЦБ-разложение матрицы Рк :[ик ,Бк ] = иБ (Рк ).

5. Сформировать вектор системы ХСк = (Хк, ту, тК ) .

6. Сформировать = ик*]ок ,

Г uk 0 0 > ГDk 0 0 1

Uak = 0 Uv 0 Da = , Dk - 0 Dv 0

V 0 0 Uw J V 0 0 Dw j

Vol. 21, No. 02, 2018

7. Сгенерировать набор сигма-точек

Civil Aviation High Technologies

ха=ха, хг = ха+у( за )г,

ХГ+ЛХа = Хк -у(^?а ) , У=д/ Nха + Я , Я = (( + р)-Nха , ' = 1,..., N^ .

8. Получить преобразованные значения сигма-точек на основании уравнения состояния: Хк = I(Хк'' ),Хк е Г, г = 0,...,Ь-1.

9. Найти прогнозное значение Хк :

— L-1 1

Xk _X Xk , _ -17T) ^ , i _ 1,..., L -1.

X k

i_0 Nx ? +1 2 (Nx a +1)'

10. Получить ЦБ-разложение матрицы Рк :[ик,Бк] = ЦБ(Рк).

11. Сгенерировать набор сигма-точек:

за=иара, х^0=хга, Хг = х^+у( за )г, хГ ха = ха -у( за )г,

у = ^ Nхa + Я , Я =£,2 (( + р) - Nха , ' = 1,..., Nха .

12. Получить преобразованные значения сигма-точек на основании уравнения наблюдения: = Е(Хк') = 0,-'Ь-1.

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

T

4J _ Xw-Tk,J, К, _ Xw- (J -ik,J)((-^k,J),

i_0 i_0 _XXk-*J

Kk+1 _ Pxy,k ((y,k ) ,

Z(+1)_ Z(j)+ K (y - Y ) 71 _ X

Zk+1 " Zk+1 + Kk+Ц Yk+1, j Yk, J j , Zk+1 " xk ,

GkJ) _ Vk Wc (Vk )T - Vk Wc {Yk,j - Yk,J }T (Pyy,k )-1 {Yk;J - Yk,J } Wc ( ), [UGJ,k,M]_ UD((j)), j _ 1,...,q, i _ 0,...,L -1.

14. Положить Sk+1 _ UkU^yDk, Zk+1 _ Z|+1

15. Сгенерировать набор частиц:

4+1 ~ n(z,zk+1,sk+x), i =1,...,np

16. Вычислить веса частиц:

p (+1, Zk+1 )n( z[+1, xk, Pk

n(-Zk+1, Zk+1, Sk+1)

I—k+1> - k

wk ^^-1 = 1,...,Np

NP Np T

17. Xk+1 w^Zi+1; pk+1 =Zwk(Zk+1 -Zk+1 )(+1 -Zk+1) .

г=1 1=1

18. Если к = N, то вычисления завершить. В противном случае положить к = к +1 и перейти к п. 4.

ЗАДАЧА СОПРОВОЖДЕНИЯ ЦЕЛИ В ПАССИВНОМ РЕЖИМЕ

Рассмотрим задачу сопровождения цели в пассивном режиме, т. е. модель, в которой наблюдениями является только пеленг (угол) сопровождаемого объекта. Такого рода задачи очень часто возникают, например, в гидролокации, когда наблюдатель не может облучать объект с целью измерения дистанции до него. Пусть на интервале времени [0,7] имеются дискретные отсчеты времени: 1к = А(к, А( = Т/ N, к = 0,...,N. Тогда в простейшем двумерном случае модель движения можно описать следующим образом:

x(tk+1) = x(tk ) + v* (tk )At +

a* (tk )A2

2

av (tk )A2

Ж+1 ) = Ж)+Vy (tk) At ++^ (k).

+ sx (k),

(4)

Наблюдателю доступны только значения пеленга:

d(tk+1) = arctan

V x(tk+1) J

+ we(k),

(5)

где ) - гауссовская белошумная помеха с известной дисперсией и нулевым

математическим ожиданием, а ех (к), гу (к) - белошумные возмущения с известными

дисперсиями а2 и а2у и нулевыми математическими ожиданиями. Начальное положение

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

Для сведения задачи (4)-(5) к модели (1)-(2) необходимо положить

Vol. 21, No. 02, 2018

Civil Aviation High Technologies

f (Xk, Vk ) =

x{tk ) + vx (tk )Ai +

ax (tk )At

+ (k)

«v (tk )A2

y(tk) + Vy (tk )At + + Sy (k)

, Xk =

x(tk )' У (tk)

, V =

** (k)" ^y (k)

и вектор-функция

g (Xk ,Wk) = arctan( y (tk) / x(tk)) + we (k), 7k = d(tk), Wk = ^ (k).

Кроме того, предполагается, что в модели (1)-(2)

X0 - Я(т0,Е0), Е0 е М2*2, m0 е М2, Ev =

а 0

0 а,2

, Е w а^2,

где т0[м] = (5000, 5000) , Е0[м] =

Г100 0 ^

v 0 100у

[м] = 3,2 ау [м] = 3,2 aw [гРаД] = 1,

А, [с] = 5, T[с] = 300, vx (tk ) [м/с] = V cos (Афк), Vy (tk ) [м/с] = v sin (Афк), ax (tk ) [м/с2 ] = 10, ay (tk)[м/с2] = 10, Ак[град] = 0,5.

Следует отметить, что только по значениям пеленга в общем случае отсутствует возможность определения координат цели, что приводит к неоднозначности рассматриваемой модели. Однако в случае наличия помехи происходит регуляризация модели и координаты цели поддаются оценке. На рис. 1, а продемонстрирована отдельная оценка траектории движения наблюдаемой цели, а на рис. 1, б указана RMS ошибка оценки расстояния между наблюдателем и сопровождаемым объектом (расчет производился для 1000 реализаций).

а б

Рис. 1. Для случая пассивной локации: оценка отдельной траектории для UD-UKF и UD-UPF фильтров (а); RMS-ошибка между истинным положением и полученным с помощью соответствующих фильтров (б) Fig. 1. Evaluation of an individual trajectory for UD-UKF and UD-UPF filters (а); RMS error between the actual position and received by means of related filters (б)

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

Civil Aviation High Technologies

Vol. 21, No. 02, 2018

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

ЗАДАЧА СОПРОВОЖДЕНИЯ ЦЕЛИ В АКТИВНОМ РЕЖИМЕ

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

Q(tk+j) = arctan

( У (tk+1) ^

+wö(k X

V x(tk+1) J

P(tk+1) = V * 2(tk+1) + У 2(tk+1) + Wp(k).

(6)

Кроме того, добавим в исходную модель (4) случайное маневрирование наблюдаемого объекта, т. е. добавим случайное изменение угла ф(1.к) направления вектора скорости:

x(tk+1) = x(tk) + v(tk )cos(^(tk ))At + «Щ^ + s* (k),

ay (tk )A2

У (tk+1) = У (tk) + v(tk )sin(^(tk ))At + Sy (k),

Ф(к+1) = Ф(Ч) + s,(k).

(7)

Далее проведем численное моделирование для рассматриваемого примера. Для проверки работоспособности построенных фильтров в реальных условиях для модели (7), на вход в качестве измерений будут поступать дистанция и пеленг для движения цели с фиксированным неизвестным для наблюдателя маневром. Маневр задается функцией ф(1к ):

фк+1) = ф(4) + Д Хк <Т0, |ф(/к+1) = ф(гк)-Дф, гк >То.

Данный маневр наглядно виден на рис. 2. На рис. 2, а, б изображены графики отдельной траектории и ЯМБ-ошибка оценки фильтрации соответственно. ЯМБ-оценка фильтрации рассчитывалась методом Монте-Карло по 1000 траекторий. Значения числовых параметров для расчетов выбирались равными:

m,

1 [м] = (5000, 5000)T , Z0[м] =

(100 0 ^

V 0 100J

ax [м] = 10,

^У [м] = 10, <ге [град] = 573, Ор[град] = 1, афград] = 0,00873, At [с] = 10, T [с] = 1200, Vx (tk ) [м/с] = 15, Vy (tk ) [м/с] = 14, «x (tk ) [м/с2 ] = 0,5, «у (tk ) [м/с2 ] = 0,5.

а б

Рис. 2. Для случая активной локации: оценка отдельной траектории для UD-UKF и UD-UPF фильтров (а);

RMS-ошибка между истинным положением и полученным с помощью соответствующих фильтров (б) Fig. 2. Evaluation of an individual trajectory for UD-UKF and UD-UDF filters (а); RMS Error between the actual position and received by means of related filters (б)

Для численного решения данных задач с помощью рассмотренных UD-UKF и UD-UPF алгоритмов выбирались следующие значения параметров: ^ = 0,5, ( = 2, p = 0.

ОБСУЖДЕНИЕ полученных результатов и заключение

Из приведенных численных результатов примера видно, что значения RMS-ошибки для UD-UKF фильтра немного меньше, чем для UD-UPF фильтра. В первом примере с ростом объема наблюдений для UD-UKF фильтра ошибка начинает уменьшаться. Во втором примере после второй части маневра ошибка постепенно начинает увеличиваться. Большую неточность UD-UPF по сравнению c UD-UKF фильтром можно объяснить недостаточным количеством частиц, моделируемых для UPF фильтра. С ростом числа частиц точность UD-UPF фильтра растет, но вычислительные затраты на моделирование этих частиц становятся весьма существенными.

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

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

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

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

3. Миллер А.Б., Миллер Б.М. Отслеживание подводной цели с использованием пеленгационных измерений // Информационные процессы. 2016. Т. 16, № 2. С. 103-111.

4. Miller A., Miller B. Tracking of the UAV trajectory on the basis of bearing-only observations. Proceedings of 53rd IEEE Conference on Decision and Control December 15-17, 2014. Los Angeles, California, USA, 2014. Pp. 4178-4174.

5. Comparison of EKF, Pseudomeasurement and Particle Filters for a Bearing-only Target Tracking Problem, in Proc. SPIE Int. Soc. Optic. Eng. / X. Lin, T. Kirubarajan, Y. Bar-Shalom, S. Maskell. 2002. Vol. 4728. Pp. 240-250.

6. Simon D. Optimal state estimation: Kalman, Hand nonlinear approaches. John Wiley & Sons, 2006. 552 p.

Civil Aviation High Technologies

Vol. 21, No. 02, 2018

7. Verhaegen M., Van Dooren P. Numerical Aspects of different Kalman filter implementations // IEEE TRANSACTIONS ON AUTOMATIC CONTROL. 1996. Vol. AC-31, No. 10. Pp. 907-917.

8. Bierman G.I. Factorization methods for Discrete Sequential Estimation. New York: Academic Press, 1977. 238 p.

9. Golub G.H., Van Loan C.F. Matrix computations. The Johns Hopkins University Press, 1996. 695 p.

10. Кудрявцева И.А. Анализ эффективности расширенного фильтра Калмана, сигма-точечного фильтра Калмана и сигма-точечного фильтра частиц // Научный Вестник МГТУ ГА. 2016. № 224. С. 43-52.

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

Кудрявцева Ирина Анатольевна, кандидат физико-математических наук, доцент кафедры математической кибернетики Московского авиационного института (национального исследовательского университета), kudryavtseva.irina.a@gmail.com.

Лебедев Максим Витальевич, кандидат физико-математических наук, доцент кафедры теории вероятностей и компьютерного моделирования Московского авиационного института (национального исследовательского университета), max_max82@inbox.ru.

APPLICATION OF MODIFIED UNSCENTED KALMAN FILTER AND UNSCENTED PARTICLE FILTER TO SOLVING TRACKING PROBLEMS

Irina A. Kudryavtseva1, Maksim V. Lebedev1

1Moscow Aviation Institute (National Research University), Moscow, Russia

The study was conducted with support of Russian Foundation for Basic Research (RFBR)

grant № 17-08-00530 А

ABSTRACT

The paper describes two modified implementations of unscented Kalman filter (UKF) and unscented particle filter (UPF) to solve nonlinear filtering problem for discrete-time dynamic space model (DSSM). DSSM is supposed to be nonlinear with additive Gaussian noise. The considered algorithm modifications are based on combination of UD-factorization of covariance matrices with sequential Kalman filter. The solution of tracking problem is illustrated for two cases. In the first case the problem of estimate of movable target coordinates from observed noised bearing is considered (a problem of passive location). In the second case the problem of an active location is described when noisy values of a distance to the accompanied object besides a bearing are available to the observer. Moreover, in the second case the motion model is extended by means of introducing a new parameter (a maneuver) such as an angle of velocity direction. To examine robustness of the considered algorithms in active target tracking problem (the second case) an arbitrary maneuver that differs from the initially given one in the motion model is considered as an observation.

Key words: nonlinear filtering, Monte-Carlo methods, UD-conversion, Unscented Kalman Filter, Unscented Particle Filter.

REFERENCES

1. Julier S.J., Uhlmann J.K. Unscented filtering and nonlinear estimation. Proc. Of IEEE, 2004, no. 3, pp. 401-422.

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

Vol. 21, No. 02, 2018

Civil Aviation High Technologies

3. Miller A.B., Miller B.M. Otslezhivanie podvodnoj celi s ispol'zovaniem pelengacionnyh izmerenij. Information processes, 2016, vol. 16, no. 2, pp. 103-111. (in Russian)

4. Miller A., Miller B. Tracking of the UAV trajectory on the basis of bearing-only observations. Proceedings of 53rd IEEE Conference on Decision and Control December 15-17, 2014. Los Angeles, California, USA, 2014, pp. 4178-4174.

5. Lin X., Kirubarajan T., Bar-Shalom Y., Maskell S. Comparison of EKF, Pseudomeasurement and Particle Filters for a Bearing-only Target Tracking Problem, in Proc. SPIE Int. Soc. Optic. Eng., 2002, vol. 4728, pp. 240-250.

6. Simon D. Optimal state estimation: Kalman, Hand nonlinear approaches. John Wiley & Sons, 2006, 552 p.

7. Verhaegen M., Van Dooren P. Numerical Aspects of different Kalman filter implementations. IEEE TRANSACTIONS ON AUTOMATIC CONTROL, 1996, vol. AC-31, no. 10, pp. 907-917.

8. Bierman G.I. Factorization methods for Discrete Sequential Estimation. New York: Academic Press, 1977, 238 p.

9. Golub G.H., Van Loan C.F. Matrix computations. The Johns Hopkins University Press, 1996, 695 p.

10. Kudryavtseva I.A. Analiz effektivnosti rasshirennogo fil'tra Kalmana, sigma-tochechnogo fil'tra Kalmana i sigma-tochechnogo fil'tra chastic [Efficiency analysis of extended Kalman filtering, Unscented Kalman filtering and Unscented particle filtering]. Scientific Bulletin of the Moscow State Technical University of Civil Aviation, 2016, no. 224, pp. 43-52. (in Russian)

INFORMATION ABOUT THE AUTHORS

Irina A. Kudryavtseva, Candidate of Physical and Mathematical Sciences, Associate Professor, Mathematical Cybernetics Chair, Moscow Aviation Institute (National Research University), kudryavtseva.irina.a@gmail.com.

Maksim V. Lebedev, Candidate of Physical and Mathematical Sciences, Associate Professor, Probability Theory and Computer Simulation Chair, Moscow Aviation Institute (National Research University), max_max82@inbox.ru.

Поступила в редакцию 03.11.2017

Принята в печать 14.03.2018

Received 03.11.2017

Accepted for publication 14.03.2018