CC BY
3Труды МАИ. Выпуск № 89
УДК 519.24
www.mai.ru/science/trudy/
Численное решение задач нелинейной фильтрации на основе
алгоритмов фильтра частиц
Волков В.А.*, Кудрявцева И.А.**
Московский авиационный институт (национальный исследовательский университет), Волоколамское шоссе, 4, Москва, А-80, ГСП-3, 125993, Россия
e-mail: vlad_ell@inbox.ru e-mail: irina.home.mail@mail.ru
Аннотация
В статье рассмотрено численное решение задачи оценивания вектора состояния по результатам косвенных наблюдений на основе алгоритмов фильтра частиц двух типов: с использованием и без использования процедуры повторной выборки. К первой группе алгоритмов следует отнести Бутстреп фильтр частиц с остаточной повторной выборкой и Бутстреп фильтр частиц с улучшенной повторной выборкой. Вторая группа включает сигма-точечный фильтр частиц и Монте-Карло фильтр частиц. Приведены подробные алгоритмы моделирования. С использованием данных алгоритмов сформирован комплекс программ, позволяющий производить сравнительный анализ результатов моделирования для приведенных алгоритмов фильтрации. Проведены серии численных расчетов для различных модельных примеров. Получены и проанализированы результаты моделирования.
Ключевые слова: нелинейная фильтрация, фильтры частиц, бутстреп фильтр частиц с улучшенной выборкой, Unscented-преобразование, сигма-точечный фильтр частиц, Монте-Карло фильтр частиц.
Введение
Идея алгоритмов фильтра частиц появилась в 1950-х годах [1]. Однако, развитие данного направления, начавшееся с работы [2], приходится на 1990-е. В [2] была формально определена и описана техника повторной выборки (resampling).
Для реализации алгоритмов фильтров частиц требуются достаточные мощные вычислительные ресурсы, что особенно заметно по возросшему количеству работ в этой области после 2000-х годов [3-6].
Фильтры частиц являются достаточно широко используемым классом численных методов. Привлекательность применения алгоритмов фильтра частиц вызвана тем, что они сочетают байесовскую реккурсивную процедуру оценивания с методом Монте-Карло. Среди приложений данного подхода можно выделить следующие: обработка видео сигналов, гидролокация и радиолокация, спутниковая навигация, робототехника.
По сравнению с другими аппроксимационными методами, которые используются для решения задач нелинейной фильтрации, например, с расширенным фильтром Калмана [7-8, 11], в основе алгоритмов фильтров частиц не используется процедура локальной линеаризации или какая-либо подобная функциональная аппроксимация.
В данной работе изложено решение дискретной задачи нелинейной фильтрации на основе алгоритмов Бутстреп фильтра частиц с остаточной повторной выборкой (Bootstrap Particle Filter with Residual Resampling), Бутстреп фильтра частиц с улучшенной выборкой (Bootstrap Particle Filter with Roulette wheel Resampling), сигма-точечного фильтра частиц (Unscented Particle Filter), Монте-Карло фильтра частиц (Monte-Carlo Particle Filter). Разработан пакет программ, с использованием которого проведены серии вычислительных экспериментов для различных модельных примеров.
Постановка задачи
Случайный процесс X (t) в моменты времени t = t0, tx,..., tk,... описывается
следующим разностным уравнением:
xk+l = f X, Vk), k = 0,1,2,..., X(to) = Xo, (1)
где Xk= X(tk)&П" - вектор состояния системы, X0 - p(x) - начальное состояние системы, f(x,v):ünxn^J" - заданная вектор-функция, Vk<=ün - дискретный
векторный белый шум с нулевым математическим ожиданием и ковариационной матрицей Ev.
Случайный процесс X(t) в моменты времени t = t0,^,...,tk,... доступен косвенным наблюдениям, удовлетворяющим уравнению:
^ = g(Xk+1,Wk+1 ), k = 0,1,2,..., (2)
где Ук еПт - вектор измерений, g(x,w)■.[}"y^m - заданная вектор-функция,
IVк е I "' - дискретный векторный белый шум с нулевым математическим ожиданием и ковариационной матрицей .
Требуется в каждый момент времени г = ^ц,...по результатам всех имеющихся к этому моменту наблюдений У/ = {у,..., Ук} получить вектор оценки Хк состояния процесса Хк c использованием Бутстреп фильтра частиц с остаточной повторной выборкой, Бутстреп фильтра частиц с улучшенной выборкой, сигма-точечного фильтра частиц, Монте-Карло фильтра частиц.
Вычислительная модель решения задачи
Алгоритмы фильтров частиц, в основе которых лежит метод Монте-Карло, оперирует с набором случайных точек, аппроксимирующих искомую плотность вероятности р(хк\Ук). В силу того, что характер р(хк\Ук) не известен, сгенерировать случайные точки, называемые частицами, затруднительно. Вводится распределение значимости д(хк\Ук) , в большинстве случаев выбираемое гауссовским, на основании которого происходит генерация частиц. Тогда, для произвольного нелинейного преобразования случайного вектора Хк, имеющего распределение р(хк|ук ), справедливо следующее:
М[И(Хк)|Ук] = {И(Хк)р(хк\ук)^ = {И(Хк) Р(^^ д(Хк|У*).
Далее, на основании полученного из распределения я(хк|ук) набора частиц Х'к, к = 1,.., N , опираясь на метод Монте-Карло и используя правило Байеса, следует:
а(Xк)=!М1А(XI) )=рЩ)-=ср(Ук|хк>р(;к'у"), (3)
где с - нормирующий множитель; ^'к = ^(Х'к)- веса частиц; р(хк|ук 1) определяется уравнением состояния (1); р(ук\хк) - уравнением измерений (2); я(хк|ук) -
распределение значимости.
Предположим, что имеется набор частиц Х'к_1з к = 1,.., Np с определенными
весами дк-1, тогда аппроксимация апостериорной плотности вероятности р (х^у-1)
для к -1 момента времени определяется следующим соотношением:
Nр
р (Хк-1 У1к-1 )«£ ^к 8( Хк-1 - Хк-1), (4)
к=1
где 8 () - дельта-функция Дирака. Тогда, для получения искомой плотности вероятности р(хк|У1к), используя уравнение Колмогорова-Чепмена [6] для р (хк|У1к-1) и д( хк|ук), необходимо соотношение (3) переписать в следующем виде:
) р(У^Хк^Р(хкIхк-1,У1к 1)Р(хк-1 У1к 1)к-1 ^
| Я ( хк | хк-1, ук ) Я (хк-11 ук-1) ахк-1
Таким образом, подставляя (4) в (5) получаем:
г р {Ук\Х'к ) р (хкХк-,1) р (Хк_1 У^-1) г р (у|Хк ) р (хк|хк_ ,)
^ Я (Хк|хк-1,У^) Я (Хк_1 у*"1) Я (гк\тк_,) , (6)
где Х'к = f{yX'k_], ivi), знак и означает пропорциональность одного выражения
NP
другому. Тогда p( x^Y*) Ч - X).
¿=1
Представленная выше методика с использованием рекуррентной формулы (6) для вычисления весов частиц носит название последовательной выборки по значимости (Sequential Importance Sampling) [3-4]. Однако, у данной методики есть существенный недостаток: при данном подходе происходит вырождение выборки. Веса всех частиц стремятся к нулю через несколько итераций, кроме одной, вес которой стремится к единице. Введение процедуры повторной выборки (resampling) позволяет решить данную проблему. В работе использованы следующие процедуры повторной выборки: остаточная повторная выборка и улучшенная повторная выборка.
Для практической реализации описанной выше методики нахождения p( xAYf)
необходимо знать р(ук\Хк),р(Х1к Х1к_1зУ1к 1) и д(Х1к Х1к_1,У1к), входящие в соотношение (6). В случае, если положить р (хк|хк_ 1,У1к"1 ) = д (Хк Х1к_ 1,У1к), то приходим к алгоритму
Бутстреп фильтра частиц. Алгоритм Бутстреп фильтра частиц, дополненный
процедурами остаточной повторной выборки [3,9] и улучшенной повторной
выборки [3], приведены ниже.
Алгоритм Бутстреп фильтра частиц с остаточной повторной выборкой 1. Положить к = 0 .
2. Сгенерировать Np частиц X0, i = 1, .., Np, где X0 ~ p(x).
-г, 1
3. Задать начальные значения весов частиц ^0, г = 1, .., Nр: ^0 =—,г = 1,.., Nр.
^р
4. Получить преобразованные значения Х\+х, г = 1,.., N на основании уравнения состояния (1): Х^ = /(Х1кX), г = 1,..,Np.
5. Вычислить веса частиц +1, г = 1,.., Ыр: 4+1 = р (у+11 Х{+1), г = 1,.., N.
6. Пронормировать полученные веса : w'k+l = , i = l,..,N
k+In ' ' p ■
z
¿=1
w'
к+1
7. Осуществить процедуру повторной выборки:
7.1. Сгенерировать = [iV • ] копий каждой частицы ,i = 1,.N ;
np
7.2. Вычислить величину остатка N = N п ;
г=1
7.3. Сгенерировать N. частиц из исходной выборки Х].+1, г = 1,.., Np с вероятностями пропорциональными N ■м^1к+1 -пиг = .
7.4. Присвоить веса частицам из нового набора W+1 =—, i = 1,.., ^^
Np
np Л ^
8. Найти ХХк+1 =Х4+1 Хк+1, 7к+1 =Х4+1 (Хк+1 -Хк+1)(Хк+1 -Хк+1) .
г=1 1 =1
9. Положить к = к+1 и перейти к п.4.
Алгоритм Бутстреп фильтра частиц с улучшенной повторной выборкой
1. Положить к = 0 .
2. Сгенерировать Np частиц Х0, г = 1, .., N, где Х0 ~ р(х).
3. Задать начальные значения весов частиц w0, i = 1,.., Np: w0 =—,i = 1,.., Np
Np
4. Получить преобразованные значения х[+1,1 = 1,..,N на основании уравнения состояния (1): х^ = /(х1кГк), i = 1,..,Np.
5. Вычислить веса частиц 4к+1, i = 1,.., Ыр: 4+1 = р (^+11 х1к+1), i = 1,.., N.
6. Пронормировать полученные веса : w'k+l = , i = l,..,N
Np ' р ■
Z
¿=1
к+1
7. Осуществить процедуру повторной выборки:
Г 1 ... N„ ^
7.1. Сгенерировать величины: ш^ех
p
vÄ ZNp J
И 0~r(o,2- max fë+1)l;
ккм V /
V 1<<Np
7.2.Выбрать частицу с номером, равным величине /'ndex, X'k"dfx и сравнить вес
W
rjndex к+1
выбранной частицы с величиной /:
7.2.1. Если > Р, то частица добавляется в новую выборку;
7.2.2. Если < р, то величина р уменьшается на вес частицы
р = р~м?1к^ и осуществляется переход к следующей частице Мех = Мех+1;
7.3. Присвоить веса частицам из нового набора 4+1 = —, i = 1,.., Ыр.
Ыр
Ыр ЫР т
8. Найти Хк+1 =Х4+1 хк+1, Ррк+1 =14+1 (хк+1 -Хк+1)(хк+1 -Хк+1) .
¿=1 1=1
9. Положить к = к+1 и перейти к п.4.
В случае, если процедуру повторной выборки заменить генерацией частиц на каждой итерации по времени из распределения значимости, а для вычисления весов использовать соотношение (3) в виде:
р (
( Ук|Хк ) р ( Хк; Хк|У1к-1)
^к--„„,. --, Х ~ д (хк
тк-1
-,Хк ~ д(Хк|У1к):
д( Хк; Хк|У1к)
то можно получить группу алгоритмов фильтров частиц, к которым относятся, рассмотренные в работе, сигма-точечный фильтр частиц и фильтр частиц Монте-Карло.
Алгоритмы сигма-точечного фильтра частиц [5-6,10,12] и фильтра частиц Монте-Карло содержат следующие основные этапы [4,10]:
- генерация набора частиц;
- оценка параметров распределения значимости д (Х^|ук). Для сигма-точечного
фильтра частиц и фильтра частиц Монте-Карло распределение значимости полагается гауссовским;
- применение процедуры фильтрации Калмана на основании параметров, полученных на предыдущем этапе;
-оценка первых двух моментов искомого распределения р(Х^|ук).
В алгоритме сигма-точечного фильтра частиц для получения оценок параметров распределения значимости применяется Unscented-преобразование. Основная идея которого заключается в том, что аппроксимации подвергается не произвольная нелинейная функция, а распределение случайной величины. К примеру, пусть имеется некоторый случайный вектор Х , который подвергается нелинейному преобразованию У = g (Х). Предполагается, что математическое
ожидание тх и ковариационная матрица Ях данного вектора X известны. Далее вводится по определенному правилу набор векторов X, ' = 1,.., N, называемых сигма-точками, таких, что их среднее и ковариационная матрица равны тх и Ях. Каждой частице ставится в соответствие некоторой вес w', ' = 1,.., N, таким образом, чтобы обеспечить несмещенную оценку математического ожидания введенного
набора частиц, причем: ^ w' = 1 .
'=1
Далее заданному нелинейному преобразованию подвергается каждая сигма-точка в отдельности. В результате получают набор преобразованных сигма-точек У', для которого вычисляют взвешенное выборочное среднее и взвешенную
N N Т
Р ~ ■ I \/ \
выборочную ковариацию: тх = ^ w'X' , Кх = ^ w' (X' - тх)(X' - тх) .
'=1 '=1
Вычисленные выборочное среднее и выборочная ковариация служат оценкой математического ожидания и ковариации исходного вектора после нелинейного преобразования.
В качестве соотношений, применяемых для нахождения сигма-точек, в данной работе рассматривались следующие [5, 8]:
X0 = тг, X' = тг + .,-гт
х' -w0 4 - -
0 0 ' 1 - w0 '+к 1 - w0 .
w = w , w =-, w =-, г = 1,...,К,
2К 2 К
К (Л^, ^ = mх-¡-КО*
где w0 - параметр, значения которого отвечают за положения сигма-точек относительно математического ожидания, (Ях)12 - г -й столбец или строка матрицы
(Ях)12, количество векторов в наборе Ыр = 2К+1, К - размерность вектора состояния.
В алгоритме Монте-Карло фильтра частиц оценка параметров распределения значимости осуществляется с использованием метода Монте-Карло на основании случайного набора частиц, генерируемого на каждой итерации из гауссовского распределения. Алгоритмы сигма-точечного фильтра частиц и Монте-Карло фильтра частиц приведены ниже.
Алгоритм сигма-точечного фильтра частиц
1. Положить к = 0 .
2. Инициализировать оценки вектора состояния системы Л0 и ковариационной матрицы Р0 в начальный момент времени.
3. Сгенерировать набор сигма-точек:
Г0 - 7 гг _ 7 + 1 К л^+К _ ^ I К
Лк = ^к, Лк = ¿к +<
^о )г2' Лк+К = ¿к Л^2
1 0 10
0 0 г 1 - w +К 1 - w .
w = w , w =-, w =-, г = 1,...,К
2К 2К
где = ^ ¿0 = р.
4. Получить преобразованные значения сигма-точек на основании уравнения состояния: Л[ = / (х[) ,г = 0,...,2К.
5. Найти прогнозное значение Хк и ковариационную матрицу ошибки
_ Л 2К _ _ 2К /_ ч/_ ^
прогноза Рк : Лк = 1 wгхk, Рк =£ wг (хк - хк)(хк -хк) .
г=0 г=0 у ' х '
6. Сгенерировать набор сигма-точек по прогнозному значению Xk вектора состояния и ковариационной матрицы Рк ошибки прогноза :
^ = Xk, К = Xk ^^ (рк )Д ^ = ^ Р
О 0 ' 1 - ™0 ''+К 1 - 0 .
= , =-, и* =-, ' = 1,...,К,
2К 2К
7. Получить преобразованные значения сигма-точек на основании уравнения наблюдения: 7к ¡ = о,..,2К.
8. Найти Ук, Ртк и Рхук по преобразованным значениям сигма-точек:
'Т.К. 2К 2К / \
'=0 '=0 '=0 ^ '
9. Выполнить коррекцию прогноза и вычислить ковариационную матрицу ошибки оценки:
Кк+1 = Рху,к (Руу,к ) ,
+1 = Xk + Кк+1 (Ук+1 - Ук ), +1 = Рк - Кк+1Руу,кКк+1.
10.Сгенерировать набор частиц: Тк+Х ~ г,¿к+1,Бк+1 ),' = 1,..,Np.
р(Ук+1,11+1 И^к+1,Xk,Рк)
11.Вычислить веса частиц: =-—^—^—г-'-,' = 1,..,N
+1, ^к+1, ^>к+1)
Np Л Np
12.Получить Xk+1 и Рк+1: ^^к+1 = 2^, Рк+1 (4+1 -4+1 )(4+1 -4+1)
'=1 '=1
13.Положить к = к+1 и перейти к п.3.
Алгоритм Монте-Карло фильтра частиц
т
1. Положить к = 0 .
2. Инициализировать оценки вектора состояния системы Л0 и ковариационной матрицы Р в начальный момент времени.
3. Сгенерировать набор точек: хк ~ N(х, хк, Рк), г = 1,..., Ыр.
4. Получить преобразованные значения сигма-точек на основании уравнения состояния: хгк = / (х^) ,г = 1,...—.
5. Найти прогнозное значение хк и ковариационную матрицу ошибки
- - 1 ^ - - 1 ^ I - - \ I - - \
прогноза Рк : хк =—хг, Рк =—• £( хк - хк)(хк - х[) .
- р г=1 — р г=1 У ' У '
6. Сгенерировать набор точек по прогнозному значению хк вектора состояния и ковариационной матрицы Рк ошибки прогноза:
7. Получить преобразованные значения точек на основании уравнения наблюдения: У к =
8. Найти Ук, Руук и Рхук по преобразованным значениям точек:
' Руу,к =тр' Рху,к =тр• ^'р г=1 — р г=1 'р г=1
9. Выполнить коррекцию прогноза и вычислить ковариационную матрицу ошибки оценки:
Кк+1 = Рху,к (Руу,к ) , ¿к+1 = хк + Кк+1 (Ук+1- Ук), ¿к+1 = Рк - Кк+1Руу,кКк+1.
10.Сгенерировать набор частиц: Z'k+l ~n(z,Zk+i,Sk+i ), i = 1,..,Np.
p(7k+i,Z'k+i)N(zk+i,Ik,Pk )
11.Вычислить веса частиц: ^ =-—^—^—г-'-, i = 1,..,N
N( Zk+i, Zk+i, Sk+i )
Np Л Np
12.Получить Xk+i и Pk+i : Xk+i = 1^, Pk+i = (Zk+i-Zk+i)(Zk+i-Zk+i)
i=i i=i
13. Положить k = k+1 и перейти к п.3.
Результаты численного моделирования
На основании изложенных алгоритмов фильтрации разработан пакет программ с использованием сред разработки MATLAB и Microsoft Visual Studio C#. Применение Microsoft Visual Studio C# позволило создать приложение, имеющее графический интерфейс для визуализации входных и выходных параметров. При помощи созданного программного обеспечения проведены серии вычислительных расчетов для различных модельных примеров. Ниже приведены результаты численного решения для следующих моделей вида (1)-(2) [10]:
1) xk+i =7% + 0.7-Vk, Yk+i = 0.6Xk+i + 0.2-Wk+i, k = 0,i,2,...;
i + Xk
2) Xk+i = 7% + (k-Xk) + 0.7-Vk, Yk+i = 0.6Xk+i + 0.2-Wk+l, k = 0,i,2,...,
i + Xk
где Х0 ~ тх,Рх) - начальное состояние системы; Ук, Жк - дискретные гауссовские
белые шумы с нулевыми математическими ожиданиями и ковариационными матрицами Еу и соответственно.
T
Для оценки эффективности результатов фильтрации наряду с графической иллюстрацией результатов моделирования производилось вычисление значений среднеквадратической ошибки в зависимости от количества запусков алгоритмов фильтрации. Значения среднеквадратических ошибок приведены в таблицах 1-2.
а б
Рис.1. Результаты моделирования системы наблюдения и оценивания, полученные на основе а -Монте-Карло фильтра частиц и сигма-точечного фильтра частиц б -Бутсреп фильтра частиц с остаточной повторной выборкой и улучшенной повторной выборкой для следующих значений параметров: х0 = 1.5, Р0 = 1, шу = 0, = 0, Еу = 1,1^, = 1.
Рис.2. Результаты моделирования системы наблюдения и оценивания, полученные
на основе а -Монте-Карло фильтра частиц и сигма-точечного фильтра частиц б -
15
Бутсреп фильтра частиц с остаточной повторной выборкой и улучшенной повторной выборкой для следующих значений параметров: Х0 = 1, Р0 = 1, шу = 0, ш,^ = 0, Еу = 3, Еw = 1.
Таблица 1 Таблица 2
СКО СКО
Количество запусков 10 50 100 Количество запусков 10 50 100
MC 1,0557 0,9772 0,7388 MC 0,9715 0,919 0,8088
UPF 0,623 0,5188 0,4658 UPF 0,4971 0,4795 0,4752
Improved 1,9915 1,9432 0,7858 Improved 0,993 0,9745 0,9713
Residual 2,0513 1,9563 0,8056 Residual 1,201 1,199 1,105
Из таблиц 1-2 видно, что алгоритмы Монте-Карло фильтра частиц и сигма-точечного фильтра частиц, не содержащие процедуру повторной выборки дают меньшую ошибку по сравнению с бутстреп алгоритмами, включающими данную процедуру. Очевидно, что с ростом количества прогонов алгоритмов фильтрации ошибка уменьшается. В целом, можно отметить, что алгоритм бутстреп фильтра частиц дает большую ошибку вне зависимости от рассматриваемой модели наблюдения, что прослеживается по данным, приведенным в таблицах 1-2 и на рис.1-2.
Библиографический список
1. Rosenbluth M.N., Rosenbluth A.W. Monte Carlo calculation of the average extension of molecular chains // Journal of Chemical Physics, 1956. Vol. 23. № 2. PP. 356-359.
2. Gordon N. J., Salmond D. J., Smith A. F. M. Novel approach to nonlinear/non-Gaussian Bayesian state estimation // IEE Proceedings-F. 1993. Vol. 140. № 2. РР. 107-113.
3. Chen Z. Bayesian filtering: From Kalman filters to particle filters, and beyond // Statistics. 2003. №1. PP. 1-69.
4. 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.
5. Julier S.J., Uhlmann J.K. Unscented filtering and nonlinear estimation // Proc. Of IEEE. 2004. №3. PP. 401- 422.
6. Julier S., Uhlmann J., Durrant-Whyte H. A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators // IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 45, № 3, 2000.
7. Пантелеев А.В., Руденко Е.А., Бортаковский А.С. Нелинейные системы управления: описание, анализ и синтез. - М.: Вузовская книга, 2008. - 312 c.
8. Simon D. Optimal state estimation: Kalman, H ^ and nonlinear approaches. John Wiley & Sons, 2006, 552 p.
9. Коновалов А.А. Основы траекторной обработки радиолокационной информации. Ч. 2. - Спб.: Изд-во СпбГЭТУ «ЛЭТИ», 2014. - 180 с.
10.Кудрявцева И.А. Анализ эффективности расширенного фильтра Калмана, сигма-точечного фильтра Калмана и сигма-точечного фильтра частиц // Научный вестник МГТУ ГА. 2016. № 224. С.43-52.
11.Колосовская Т.П. Субоптимальный алгоритм оценивания и параметрической идентификации для навигационных систем летательных аппаратов и других подвижных объектов на основе информации магнитного поля Земли // Труды МАИ, 2016, №88: http://www.mai.ru/science/trudy/published.php?ID=70666
12. Кишко Д.В. Анализ точности определения собственных координат при использовании радионавигационной системы с малыми базами между передатчиками // Труды МАИ, 2014, №78: http://www.mai.ru/science/trudy/published.php?ID=53755
