Численные методы нелинейной фильтрации для обработки сигналов и измерений Текст научной статьи по специальности «Математика»

Вычислительные технологии

Том 21, № 4, 2016

Численные методы нелинейной фильтрации для обработки сигналов и измерений

М.В. Куликова, Г. Ю. Куликов

Центр прикладной математики, Высший технический институт, Университет г. Лиссабона, Португалия

Контактный e-mail: maria.kulikova@ist.utl.pt, gkulikov@math..ist.utl.pt

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

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

Введение

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

© ИВТ СО РАН, 2016

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

Альтернативный подход, позволяющий существенно сократить вычислительную сложность при решении поставленной задачи, состоит в вычислении лишь ограниченного числа характеристик стохастического процесса. В частности, наиболее популярными среди современных алгоритмов нелинейной фильтрации являются методы, предполагающие нормальное распределение неизвестного вектора состояния стохастической модели и базирующиеся на вычислении только первых двух моментов, т. е. математического ожидания и ковариации. Фильтры, полученные в рамках этого подхода, относятся к так называемым методам нелинейной калмановской фильтрации или "методам нелинейной фильтрации, основанной на теории оптимального линейного оценивания"2 [3]. В отечественной литературе они называются субоптимальными [4, гл. 3]. Однако в отличие от процитированной монографии, где утверждается, что "численное решение этих уравнений в задачах реального времени тоже невозможно, так как для этого требуется много времени, а решать их необходимо каждый раз после получения результатов наблюдений" [4, с. 372], в алгоритмах нелинейной калмановской фильтрации дополнительно предполагается нормальность апостериорного распределения вектора состояния стохастической модели, что полностью исключает необходимость явного интегрирования дифференциального уравнения Фоккера — Планка в частных производных, а значит, такие фильтры пригодны для вычислений в реальном времени. Далее в работе все внимание будет уделено именно этому направлению в теории оценивания неизвестного вектора состояния стохастических систем.

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

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

хНеобходимо уточнить, что в отечественной литературе задача фильтрации, как правило, формулируется для стохастических систем с непрерывными измерениями (см, например, [4, разд. 2.1.4]). Последнее предполагает одновременное решение стохастических уравнений для динамики системы и процесса измерений. Решение этой более сложной задачи определяется уравнением Кушнера — Стратоно-вича или уравнением Дункана — Мортенсена — Закаи. Однако эта задача не имеет отношения к изучаемым в настоящей статье фильтрам (см. подробное объяснение в [6]) и далее не упоминается.

2От англ. "nonlinear filtering through linear estimation theory".

3От англ. Extended Kalman Filter (EKF).

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

На современном этапе в развитии методов нелинейной калмановской фильтрации можно выделить три основных направления. Прежде всего, это сигма-точечный фильтр Калмана (UKF-алгоритм5), впервые предложенный в 1995 г. [9] и затем усовершенствованный [10-12]. Во-вторых, так называемый интерполяционный фильтр с центральными разностями (CDF-алгоритм6), разработанный одновременно двумя независимыми группами исследователей в 2000 г. [13, 14]. И, наконец, подход, основанный на численной аппроксимации многомерных вероятностных интегралов специального вида (напомним, что рассматриваются только гауссовы системы), т.е. квадратурный фильтр Калмана (QKF-алгоритм7) [15] и кубатурный фильтр Калмана (CKF-алгоритм8) [3]. Важно подчеркнуть, что все вычислительные процедуры, разработанные в рамках этих трех основных направлений, не требуют вычисления матрицы Якоби и/или Гессе в отличие от обобщенного фильтра Калмана, в связи с чем они выделены в единое семейство фильтров, названных в иностранной литературе "derivative-free Kalman filters" (см. обсуждение в [16]).

В настоящее время существует огромное количество перспективных алгоритмов фильтрации в рамках каждого из перечисленных выше направлений. Отметим лишь некоторые из них. Например, реализации сигма-точечного фильтра [17-19], фильтр разделенных разностей (DDF-алгоритм9) [20], реализации квадратурного фильтра Калмана [21] и кубатурного фильтра [22, 23]. Активно развиваются аналогичные численные методы и для решения задачи сглаживания (см., например, [24] и многие другие). К сожалению, перечислить все работы, вносящие вклад в данную область исследований, не представляется возможным. В наши дни она переживает бурное развитие, о чем свидетельствует большое количество публикаций в российских и иностранных периодических научных изданиях.

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

4От англ. iterated Extended Kalman Filter (iEKF).

5От англ. Unscented Kalman Filter (UKF).

6От англ. Central Difference Interpolation Filtering (CDF).

7От англ. Quadrature Kalman Filter (QKF).

8От англ. Cubature Kalman Filter (CKF).

9От англ. Divided Difference Filter (DDF).

10В иностранной литературе "deterministic sampling approach".

11В иностранной литературе "unscented transformation".

дискретной стохастической системы, чтобы получить прогнозные значения первых двух моментов апостериорного распределения случайной величины. Отметим, что, в отличие от обобщенного фильтра Калмана, в UKF-методе нелинейная функция системы не изменяется, т. е. не линеаризуется. Отсюда вытекает основное преимущество UKF-фильтра, которое заключается в более высоком порядке аппроксимации оценки математического ожидания случайного вектора состояния системы при той же вычислительной сложности, что и в обобщенном фильтре Калмана. Доказано, что сигма-точечный фильтр обеспечивает второй порядок аппроксимации указанной выше величины для любой достаточно гладкой нелинейной функции системы. В случае, если функция плотности вероятности состояния системы является симметричной, то UKF обеспечивает третий порядок аппроксимации (см. доказательство в [25, с. 269]). Особая роль сигма-точечного фильтра объясняется еще и тем фактом, что некоторые современные методы нелинейной калмановской фильтрации имеют тесную связь именно с UKF-алгоритмом (см., например, обсуждение в [3, 13]).

Следует отметить, что UKF, как и другие перечисленные выше современные методы нелинейной калмановской фильтрации, развиты для дискретных стохастических динамических систем. Поэтому, чтобы использовать их для оценивания стохастических дифференциальных моделей, необходимо прежде всего применить тот или иной метод дискретизации и свести задачу к виду, допускающему применение обсуждаемых фильтров. На этом этапе вносится ошибка дискретизации, которая оказывает существенное влияние на работоспособность методов фильтрации. В частности, в [26] впервые разработан кубатурный фильтр Калмана для стохастических дифференциальных систем с дискретными измерениями (CD-CKF-алгоритм12), который основан на дискретизации Ито — Тейлора13 порядка 1.5 для аппроксимации стохастических дифференциальных уравнений типа Ито. Этот фильтр протестирован на задаче определения координат и скоростей самолета, осуществляющего маневр в горизонтальной плоскости. Указанная задача описывается системой стохастических дифференциальных уравнений седьмого порядка и считается достаточно сложной и пригодной для адекватного тестирования алгоритмов нелинейной фильтрации [26, с. 4985]. Дополнительно в процитированной статье проведен сравнительный анализ предложенного CD-CKF-метода с сигма-точечным и обобщенным фильтрами Калмана. По результатам вычислительных экспериментов сделан вывод о существенном превосходстве нового алгоритма нелинейной фильтрации, чем и объясняется его популярность. Однако условия модельного эксперимента нельзя считать корректно поставлеными. В частности, в рамках сигма-точечного фильтра для дискретизации стохастических дифференциальных уравнений авторы используют схему более низкого порядка, чем для разработанного ими нового кубатурного фильтра. Более того, для CD-CKF вводят дополнительное разбиение (равномерную сетку) на каждом интервале наблюдений. Другими словами, вычислительные эксперименты [26] проведены таким образом, что выводы о существенном преимуществе CD-CKF-алгоритма становятся очевидны.

Целью данной работы является построение сигма-точечного фильтра Калмана для оценивания вектора состояния стохастической дифференциальной системы с той же самой схемой дискретизации порядка 1.5, что и в кубатурном фильтре CD-CKF [26]. Для проведения сравнительного анализа этих фильтров используется упомянутая выше тестовая задача, т. е. задача отслеживания координат и скоростей самолета, осуществ-

12От англ. Continuous-Discrete Cubature Kaiman Filter (CD-CKF).

13 В иностранной литературе "Ito-Taylor discretization".

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

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

Кроме того, в статье обсуждаются особенности практической реализации современных нелинейных алгоритмов калмановской фильтрации, а также их работоспособность для различных комбинаций угловой скорости маневрирующего объекта (самолета) и времени ожидания наблюдений. Для дискретных стохастических динамических систем подобный сравнительный анализ современных методов нелинейной калманов-ской фильтрации можно найти и в отечественной литературе. Например, отметим анализ среднеквадратической погрешности определения координат объекта в бесплатформенной инерциальной навигационной системе [27] и сопоставление методов нелинейной фильтрации при оценивании параметров радиолокационных систем [28].

1. Постановка задачи нелинейной фильтрации и БХЕ как классический метод ее решения

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

со случайным начальным условием х(Ь0) ~ М(х0, По). Здесь х(£) е М — неизвестный вектор состояния системы14; ¥ : М х М ^ М — нелинейная вектор-функция; С — постоянная квадратная матрица размера п и @(£) — п-мерный процесс броуновского движения с постоянной диффузией > 0, Е Мгахга. Далее будем рассматривать стандартный винеровский процесс с независимыми компонентами, т. е. Q = 1п, где 1п — единичная матрица размера п.

Дополнительно динамическая система (1) снабжена измерителем, позволяющим получать данные о состоянии этой системы в дискретные моменты времени, а именно через определенный промежуток времени 8 = Ьк — Ьк-1, где к — дискретное время нашей задачи, в виде последовательности векторов хк. Полученная информация связана с вектором состояния исходной системы через функцию Ь, т.е.

где хд. — значение вектора состояния хежас4(£) в момент времени Ьк. В дискретном измерителе (2) вектор хк Е обозначает доступный для наблюдения вектор измерений,

с!х(г) = ¥ (х(г),г) ¿г + Gdp(t), г > го,

(1)

ък = Ь(хк,гк) + vfc, к > 1,

(2)

14В работе реализация решения уравнения (1) обозначена как хежаС((£).

h : Rn х R ^ Rm — линейную или нелинейную вектор-функцию и {vi, v2,... } — нормально распределенную последовательность шумов с нулевым средним и ковариационной матрицей R > 0, R Е Rmxm, соответственно. Предполагается, что последовательности шумов в стохастической модели (1), (2) не зависят от начального вектора состояния системы x(tо) и взаимно некоррелированы.

Задача фильтрации для системы (1), (2) заключается в вычислении оптимальной (субоптимальной) оценки неизвестного вектора состояния системы, обозначенной далее Xfc|fc в момент времени tк, на основе доступных измерений Z\± = {z1,... , Zk} и с априорными условиями x(t0) ~ N(хо, По). В настоящей работе рассматривается задача построения субоптимального фильтра для нелинейных стохастических систем (1), (2). Ее классическим решением является обобщенный фильтр Калмана, известный уже в 70-х гг. прошлого века [1]. Ниже рассмотрим этот метод более подробно.

Байесовский фильтр для оценки неизвестного вектора состояния нелинейной стохастической системы состоит из двух этапов: этапа экстраполяции и этапа обработки измерений. На этапе экстраполяции необходимо вычислить прогнозную оценку вектора состояния системы (1), (2). Для этого стандартный EKF-алгоритм предполагает применение метода Эйлера для приближенного решения стохастического дифференциального уравнения (1) на каждом временном интервале (t, t + 8), т. е. на интервале между двумя последовательными поступлениями измерений. Таким образом, применив этот метод, получаем

x(t + S) = x(t) + f (x(t), t) + Gw(t),

где w(t) ~ N(0, 8In), поскольку fl(t) в уравнении (1) есть n-мерный стандартный вине-ровский процесс с независимыми компонентами. Принимая во внимание, что неизвестный вектор состояния x( ) из дискретного уравнения, приведенного выше, не зависит от шумов в системе (1), (2), из последнего соотношения вытекает

E [x(t + 5)] = E [x(t)] +8 E [f (x(t), t)], (3)

var [x(t + 5)] = var [x(t) + f (x(t), t) ] +SGGT. (4)

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

Допустим, что в момент времени tk-1 известны оценка и, соответственно,

матрица ковариации ошибки оценивания Рк_цк_1. Необходимо вычислить предсказание для момента времени tk = tk-1 + 8, т. е. найти и Рк\к_1. Тогда имеем

f (x(t), t) « f (xfc-1|fc-1,4-1) + df ^l1^1 tk-1 (x(t) - x*_1|fc_1) . (5)

Подставляя разложение (5) в уравнения моментов (3), (4) и принимая во внимание, что tк_1 = t, tк = t + 8 и, соответственно, x(t) = х^_1|^_1, получаем этап экстраполяции стандартного EKF-алгоритма в виде следующих формул (см. также [1]):

xfc|fc_1 = Xfc_1|fc_1 + (Xfc_1|fc_b tк_1) , = ( + df (Xfc_1|fc_1, tk_1)\ ( .df (Xfc_1|fc_1, tk_1)\T + T

Pklk_1 = [In +6 Щё) ) Pk_1lk_1 [In +6 ЩГ) ) +6GG .

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

Развитие численно устойчивых по отношению к ошибкам округления квадратно-корневых алгоритмов является одной из важнейшых задач современной теории калма-новской фильтрации [29, 30]. Здесь кратко подчеркнем основные принципы построения таких реализаций и их преимущества. Все квадратно-корневые методы основаны на разложении Холецкого15 матриц ковариации фильтра. Разложение осуществляется только один раз, т. е. только для начальных матриц ковариации. Далее обновление уравнений фильтра происходит только в терминах треугольных матриц, которые являются квадратными корнями исходных. При этом стандартная и квадратно-корневая реализации алгебраически эквивалентны.

Конечно, подобная организация вычислений не свободна от влияния ошибок округления, однако она гарантирует сохранение теоретических свойств матриц ковариации на практике и обеспечивает двойную точность вычислений. В свою очередь, идея построения ортогональных квадратно-корневых фильтров состоит в использовании численно устойчивых ортогональных преобразований16 на каждом этапе фильтрации для обновления квадратных корней исходных матриц в соответствии с уравнениями вида ЯЯТ = ААТ ± ВВТ (см. подробнее [31-36]). Ортогональные квадратно-корневые фильтры имеют ряд дополнительных преимуществ. Здесь отметим лишь то, что в силу простой и компактной записи ортогональные методы ориентированы на параллельные вычисления. Их "часто намного проще описать и реализовать (как на программном, так и на физическом уровне), чем систему явных уравнений фильтра" [29, с. 428].

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

2. Современные методы нелинейной калмановской фильтрации

2.1. Сигма-точечный фильтр Калмана для дискретных стохастических систем

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

15 В работе используется разложение Холецкого вида А = А1/2АТ/2, где А1!2 — нижняя треугольная матрица. Приняты следующие обозначения:

АТ/2 = (^1/2)Т, А-111 = (А1/2)-1, ¿-Т/2 = (^-1/2)Т.

16Рассматриваются преобразования вида QA = Д, где Q — ортогональное преобразование, приводящее к нижнему треугольному виду матрицу А, т.е. Д — нижняя треугольная матрица.

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

Первый вариант сигма-точечного фильтра Калмана был разработан для дискретных стохастических систем [8-10]. Итак, рассмотрим уравнение вида

Хк = ¥ (хк-1, гк-\) + Gwfc-l. (6)

Здесь хк € ММп — неизвестный вектор состояния системы; ¥ : Мп х М ^ Мп — нелинейная вектор-функция; С — постоянная матрица размера п х п и {wk} — дискретный белый шум, где Wk ~ N(0, 81п), т. е. предполагается, что уравнение (6) получено в результате дискретизации стохастического дифференциального уравнения (1). Как и ранее, эта система снабжена измерителем вида (2). Последовательности {wk} и {\к} не зависят от начального вектора состояния системы хо ~ N(Хо, По) и взаимно некоррелированы.

ИКР-метод для задачи (6) с измерителем (2) основан на специальном (детерминированном) выборе 2 п + 1 сигма-точки (т.е. 2 п + 1 сигма-вектора X¿) вокруг полученной оценки вектора состояния системы на каждой итерации фильтра. Другими словами, пусть в момент времени Ьк известна случайная величина со средним Хк|к и ковариацией Рцк. Тогда сигма-векторы определяются следующим образом:

X о = Хк|к,

^г = Хк|к + л/пГ\ р]1^, г = 1,... ,п,

х г = Хк|к - л/п + Л р^-/п1;к|к, г = п +1,..., 2п, где Р^к^к означает г-й столбец Р]]2. Сигма-векторы (7) имеют весовые коэффициенты

^0М = ^ = +1 — а2 + /,

Л + п Л + п

= ^ = 27Л~~п), ъ = 1,..., 2п, (8)

2( Л + п)

которые используются для аппроксимации среднего (коэффициенты ^(т)) и ковариа-ции (коэффициенты ^(с)) заданной случайной величины.

В формулах (7), (8) свободный параметр а определяет разброс сигма-точек вокруг среднего. Обычно он выбирается в границах 10-4 < а < 1. Параметр / позволяет принять во внимание априорные данные о функции плотности вероятности неизвестного вектора состояния системы. В частности, было доказано, что / = 2 является оптимальным для нормального распределения [37]. И, наконец, Л = а2(п + к) — п — параметр масштабирования, который зависит от еще одного свободного параметра к, обычно полагаемого равным 3 — п. Напомним, что п означает размерность оцениваемой стохастической системы (6).

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

1) классический UKF [11] с а = 1, ¡3 = 0, к = -4;

2) UKF с а = 10-3, р = 2, к = 0;

3) параметризация, которая приводит к уравнениям кубатурного фильтра Калмана, т. е. с а =1, ¡3 = 0, к = 0.

Подчеркнем, что указанные значения параметров приведены для рассматриваемой в статье модели маневрирующего летательного аппарата, т. е. при п = 7.

Как сказано выше, сигма-точки (7) и их весовые коэффициенты (8) применяются для аппроксимации функции плотности вероятности, скажем П(х), n-мерной случайной величины х со средним х^ и ковариацией P^k по формуле

2 га

г(т)

П(х) W(m)S(x - *г), (9)

i=0

где $(•) — дельта-функция Дирака. Таким образом,

E [f (х)] = У f (х)П(х^х « ^ W<f)i {X) , (10)

R" г=°

var[f (х)] = / (f (х) - E[f (х)]) (f (х) — E f (х)]) П(х)^х

2 га

Е ^ (f (Х*) - E[f (х)]) (f (Xг) - E[f (х)])т . (11)

г=0

В работе [11] показано, что 2п + 1 — это минимальное количество априорно выбранных (детерминированных) сигма-точек, которое необходимо, чтобы первые две важнейшие числовые характеристики (моменты) теоретического распределения были равны соответствующим статистическим характеристикам. Более того, в [25, с. 269] доказано, что в случае симметричной функции плотности вероятности состояния системы иКГ-метод обеспечивает третий порядок аппроксимации для оценки математического ожидания для любой нелинейной достаточно гладкой функции ^ (хк-1,1к_1) системы (6).

Алгоритмы 3 и 4, приведенные в приложении настоящей статьи, представляют собой

стандартную и ортогональную квадратно-корневую версии сигма-точечного фильтра

Калмана (см. также [25, с. 233, 275]). Прежде всего, обратим внимание, что в отличие

от ЕКР-метода стандартная реализация ИКР (алгоритм 3) требует разложения Холец-

кого на каждой итерации фильтра, поскольку квадратные корни матриц ковариации 1/2 1/2

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

Из сказанного выше вытекает основной недостаток сигма-точечного фильтра Кал-мана — невозможность построения его эквивалентного квадратно-корневого варианта,

(с)

поскольку весовой коэффициент W0( ) может быть отрицательным и, следовательно, квадратный корень для него не определен над полем вещественных чисел. С другой стороны, в зарубежной литературе можно встретить большое количество работ, посвященных квадратно-корневым реализациям сигма-точечного фильтра Калмана (см., например, работы из списка литературы и многие другие). Алгоритм 4 (см. приложение) представляет собой один из наиболее популярных квадратно-корневых фильтров, разработанный в [25, с. 275].

Однако многие квадратно-корневые реализации UKF, встречающиеся в литературе, на самом деле являются псевдоквадратно-корневыми, поскольку они алгебраически не эквивалентны стандартной реализации UKF [3]. В их основе лежит достаточно простая

(с)

идея. Весовой коэффициент W0( ) и соответствующий ему сигма-вектор обрабатываются отдельно и используются с целью выполнения малоранговой модификации квадратных корней матриц ковариации Рцк-1 и Рщ2. В алгоритме 4 этому соответствует операция cholupdate17. При малоранговой модификации квадратных корней матриц

(с)

принимается во внимание знак коэффициента W0( ), а квадратный корень извлекается из абсолютной величины коэффициента. На этом этапе нарушается эквивалентность между стандартной реализацией фильтра и его квадратно-корневым аналогом. Более того, операция cholupdate не применима к матрицам Рк\к-1 и Рк\к, которые не являются положительно определенными. В силу влияния ошибок округления подобная ситуация достаточно часто встречается на практике. В случае невозможности выполнить операцию cholupdate происходит сбой алгоритма и аварийная остановка вычислений.

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

2.2. Кубатурный фильтр Калмана для оценивания вектора состояния дифференциальных стохастических систем

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

Пусть S — квадратный корень матрицы Р = SST. Тогда малоранговая модификация матрицы Р есть Р ± sjvuuT, что на языке матричных вычислений MATLAB выполняется оператором S = cholupdatejS', u,

18От англ. "the curse of dimensionality".

В случае СИГ используется кубатурное правило Гаусса третьего порядка для вычисления вероятностных интегралов вида

г. 2п

Е р(х)] = ^ Р (х) Я (х; Д, Е) dx « — ^ Р (д + Е1/2|г) . (12)

Здесь М (х; д, Е) — функция плотности вероятности нормального распределения случайного вектора х со средним /л и ковариациеи Е = Е1/2Ет/2 и ^ — узлы кубатурной формулы (12), которые определяются следующим образом:

I /nei, 1=1^ .. ^ ( Л

= 1 „• _ ^ | 1 о,» (13)

{

— у7nei-n, i = п + 1,..., 2п,

где e — г-й координатный вектор в пространстве Rn.

Теперь нетрудно обнаружить тесную связь между UKF и CKF. Так, если в уравнениях сигма-точечного фильтра Калмана (7), (8) положить a = 1, [3 = 0, к = 0, то получим кубатурные точки (13) с весовыми кэффициентами 1/(2п). Всего 2 п кубатурных точек, поскольку коэффициенты W0(m) = W0(c) становятся равными нулю. Более того, с учетом предположения о нормальности распределения вектора состояния стохастической системы уравнения для первых двух моментов кубатурного и сигма-точечного фильтров, очевидно, совпадают (сравнить, например, (10) и (12) и аналогичные формулы для матрицы ковариации).

Таким образом, UKF-метод является более общим и гибким инструментом для оценивания вектора состояния нелинейных стохастических моделей, чем кубатурный фильтр Калмана, который, можно сказать, в некотором смысле является частным случаем UKF. Однако существенное преимущество кубатурного фильтра над UKF-алгоритмом — это наличие эквивалентной квадратно-корневой реализации, так как W0 = W0(c) = 0, и тем самым проблема отрицательных весов, присущая сигма-точечному фильтру, для кубатурного фильтра отсутствует.

CKF впервые был разработан [3] для оценивания неизвестного вектора состояния дискретной стохастической системы (6) с наблюдателем (2). Позднее, в [26], предложено его обобщение на стохастические модели с непрерывным временем вида (1), (2) и показано существенное преимущество в точности оценивания над соответствующими EKF- и UKF-алгоритмами. Поэтому остановимся более подробно на CD-CKF [26].

Как сказано выше, этот кубатурный фильтр основан на разложении Ито — Тейлора порядка 1.5 для дискретизации стохастического дифференциального уравнения (1), которое имеет вид

S2

x(tk + S) = x(4) + ôf (xfc, tk) + yL0f(xfc, tk) + Gw + (Lf (xfc, tk)) y, (14)

4-V-'

f d(xk ,tk)

где S = tk+i — tk. С целью удобства дальнейшего изложения мы ввели обозначения f (xk, t k ) = f (x( t k ), t k ) и f d(xk, tk ) для указания на нелинейную функцию в полученной дискретной стохастической системе (14). Пара w, y — коррелированные гауссовы шумы с нулевым средним, т.е. w ~ M(0,#In), y ~ М(0, (ô3/3)In) и E[wyT] = (52/2)In.

Два дифференциальных оператора Ь0 и Ь определяются следующим образом:

г) п Я 1 п Я2

Ь0 = ш + £ ^ + 2 ^, (15)

г=1 ¿,р,г=1 '

п В

Ь = ^ з = 1,...,п. (16)

г=1 г

В итоге обозначает матрицу размера п х п, где (г,])-й элемент есть величина ^ (г,] = 1,... ,п) (см. подробности в [26, с. 4980]).

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

= Е[х(^ + 8)^1*] = Е[^(х*., гк)1г1±] . (17)

Далее, для приближенного вычисления правой части уравнения (17) используем куба-турное правило Гаусса третьего порядка (12), т.е.

/1 2п

f4 (хк, 4) Я (хк; Хк\к, рк\к) ¿хк ^ — ^ fЛ + Рк\к&, ,

К" г=1

где — кубатурные точки из формулы (13).

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

Рк+1\к = Е[(х(4 + 8) — )(х(Ьк + 8) — )т|^] = = Е (хк, Ь к ^т(хй, tk — хк+1\к Хт+1|£ +

+ I3 Е [(Ы (хк, Ьк)) (Ы (хк, Ьк))т ^] + Е [(Ы (хк, Ьк))т 1г1±] +

+ ^ Е[(Ы (хк,Ьк)) ] Ст + 8ССт. (18)

Численная аппроксимация математических ожиданий в правой части формулы (18) опять осуществляется с помощью кубатурного правила Гаусса третьего порядка. Для этого введем обозначения для кубатурных векторов X¿,к\к = ххк\к + Рщ^ и их прогнозных значений X*щк = ¡¿(X, Ьк). Из последних векторов составим матрицу Х^ размера п х 2п со столбцами вида X*к\к — Хк\к, %= 1,..., 2п. Тогда равенство (18) с учетом принятых обозначений дает формулу для приближенного вычисления матрицы ковариации (см. подробности в [26]), т.е.

1 т 83 т

« — Х*|, (Х*\,) + 6сст + 3 (ы (хк\к,гк)) (ы (х^,гк)) +

+ 6!2(с (Ы (Xfc|fc,4))т + (Ы (Хк\к,и)) Ст) . (19)

Более того, чтобы повысить точность предсказания среднего и матрицы ковариации ошибки, на каждом интервале [Ьк, гк+1] длины 8 = Ьк+1 — гк вводится постоянная равномерная сетка из т подотрезков. Зетем дискретизация уравнения (1) с помощью схемы

Ито — Тейлора порядка 1.5 проводится отдельно на каждом из подотрезков ¿к'+1)], где = Ьк + 3т, з = 0,..., т, и т = 5/т [26].

Таким образом, на этапе экстраполяции используется т-шаговая процедура обновления оценки вектора состояния и матрицы ковариации. Алгоритм 5, приведенный в приложении настоящей статьи, представляет собой стандартную реализацию куба-турного фильтра Калмана [26], в то время как его численно устойчивый (по отношению к ошибкам округления) квадратно-корневой аналог изложен там же в виде алгоритма 6. Как отмечено выше, кубатурный фильтр Калмана допускает существование эквивалентной квадратно-корневой реализации, т. е. алгоритмы СВ-СКЕ и БИ СВ-СКЕ алгебраически эквивалентны, что чрезвычайно важно для корректного функционирования этого метода калмановской фильтрации.

3. Новые методы нелинейной калмановской фильтрации

для оценивания стохастических дифференциальных систем

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

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

3.1. Сигма-точечный фильтр Калмана для оценивания стохастической дифференциальной системы

Итак, применяя разложение Ито — Тейлора порядка 1.5 для дискретизации стохастического дифференциального уравнения (1), приходим к дискретной задаче (14), где дифференциальные операторы Ь0 и Ь определены формулами (15) и (16) соответственно. Шумы у в (14) имеют нулевое математическое ожидание, а следовательно, с учетом принятых выше обозначений справедливо соотношение (17). Далее, для вычисления правой части формулы (17) используем представление (9) для аппроксимации функции плотности вероятности П(х) п-мерной случайной величины х со средним Хк|к и кова-

риацией Рк|к (которые известны в момент времени ¿д) с помощью 2п +1 сигма-вектора Хг из формулы (7) с весовыми коэффициентами (8). По аналогии с (10) имеем

Е [У¿(хк, 4)|^1:к] = fл (хк, tк)П (хк; Хд|к, Рщд) <1хд л (Хг, ^).

2га

(т).

г=0

Опять же с учетом использованной схемы дискретизации для матрицы ковариации справедливо равенство (18). Для приближенного вычисления соответствующих интегралов в нем снова применим аппроксимацию (9) для функции плотности вероятности П(х) п-мерной случайной величины х со средним Хд|к и ковариацией Рд|к с помощью 2п +1 сигма-вектора Xк, найденного по формулам (7), с весовыми коэффициентами (8). В итоге, положив X* дд = г,к\к, tк), и аналогично (19) находим

2га

Рк+1\к « £ ^(с) (^,к|к - Хк|к) (л&|к - хк|к)т + бсст+ =0

б2 г„,.„,- . ччт „„,. ¿3

т о т

С (Хк|к,4)) + (Хк|к,Ст + у (Хд|к, (М (х^д,.

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

В заключение отметим, что разработанный фильтр СВ-ИКЕ алгоритмически мало чем отличается от аналогичного кубатурного фильтра [26] (сравнить алгоритмы 5 и 7). Поэтому в большинстве случаев мы может ожидать одинаковое качество оценивания состояния стохастической системы (1), (2). Однако наличие свободных параметров в СВ-ИКЕ позволяет дополнительно оптимизировать этот фильтр под конкретную прикладную задачу, что может повысить его эффективность на практике. С другой стороны, напомним, что квадратно-корневая реализация СВ-ИКЕ (т.е. алгоритм 8) не эквивалентна его оригинальной версии, в отличие от квадратно-корневого аналога СВ-СКЕ (см. алгоритм 6 в приложении статьи), что может нивелировать указанное преимущество. Поэтому выбор наилучшего в той или иной ситуации алгоритма фильтрации должен осуществляться с учетом особенностей прикладной задачи.

3.2. Обобщенный фильтр Калмана для оценивания стохастической дифференциальной системы с дискретизацией Ито — Тейлора порядка 1.5

Как показано в разд. 1, классический ЕКЕ основан на стохастическом методе Эйлера порядка 0.5 для дискретизации уравнения (1). Чтобы повысить точность вычислений, заменим его на разложение Ито — Тейлора порядка 1.5. Тогда справедливо равенство (14), где дифференциальные операторы Ь0 и Ь определены в (15) и (16). Аналогично формулам (17), (18) вместо (3), (4) имеем

82

Е [х(* + 5)]= Е ^а(х(1), 1)] = Е +5 Е ^ (х(*), I)] +- Е (х(*), I)], (20) уаг[х(£ + £)] = Е^а(х(1), ^Т(х(г), г)} — Е[х(* + 5)] Е [хт(г+ 5)] +

+ у Е[(Ы(х(1), Ь))(Ы(х(1), ¿))т] (х(1), ¿))т] +

+ у Е[(Е£(х(1), *))] Ст + 5ССт

(21)

Для вычисления правых частей формул (20), (21) ЕКЕ предполагает разложение нелинейной функции ^ (х(1), Ь) в ряд Тейлора до членов первого порядка включительно (см. разд. 1). Допустим, что в момент времени Ьк_1 известны оценки и Рk_1|k_1

и требуется осуществить предсказание для момента времени Ьк = Ьк_1 + 5, т.е. найти прогнозные значения и Рк\к_1. Подставляя разложение (5) в уравнения момен-

тов (20), (21) и принимая во внимание, что Ьк_1 = ¿, tк = 1 + 5 и, соответственно, х(Ь) = х£_1|£_1, х(, + 8) = получаем этап экстраполяции нового ЕКЕ-метода в

виде следующих формул:

хй|й_1 = Xk_1|k_1 + ^ (Xfc_1|fc_1, Ък_1) + (хк_Цк_1, ^к_\) = fd(Xk_1|k_1, ^к_1),

(Xk_l|k_l,4_1)

т

д х(г)

+ у (Xk_l|k_l, гк_1)) {хk_l|k_l, г+

дх( )

<^2

+ у (с (Ы (х^_1|^_1,+ (Ы (х&_1|^_1,и_1)) Ст) .

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

4. Вычислительные эксперименты

Протестируем перспективные алгоритмы нелинейной калмановской фильтрации на известной задаче отслеживания координат и скоростей самолета, осуществляющего маневр в горизонтальной плоскости [26]:

ё 0 0 0 0 0 0 0

ё — ш Г] 0 0 0 0 0 0

Г] 0 0 0 0 0 0 0

V = ш ё (И + 0 0 0 (Г1 0 0 0

С С 0 0 0 0 0 0 0

С 0 0 0 0 0 0 0

ш 0 0 0 0 0 0 0 &2

(22)

где вектор состояния системы х(Ь) = [е, ё,г], г],(, С,ш]т имеет следующие компоненты: £,Т],С — координаты самолета, ё, г], £ — значения скоростей в направлении соответствующих координатных векторов, аш — угловая скорость самолета или скорость выполнения

поворота. В уравнении (22) ^(¿) означает 7-мерный стандартный процесс броуновского движения. Кроме того, о1 = и о2 = 0.007. Временной интервал наблюдений выбран равным [0, 210 с].

Следящая система состоит из радара, расположенного в начале системы координат и оборудованного для измерения дальности г, азимута в и угла подъема ф над поверхностью Земли. Другими словами, в момент времени Ьк доступная информация задается вектором измерений ъ д = [г к, в к ,фк ]т. Данные поступают в зашумленном виде и через временной интервал 5 = Ьд — tк-1. Они связаны с координатами самолета следующими нелинейными соотношениями:

Гк 2 + + С!

дд = аг^ (г]к/ед) / \ + V!, V! -Я(0,Я),

. Фк . аг^ ск Ы 4 + VI)

diag( а"2

о,

2 аф) с °2

(23)

50 м, ае = 0.1е

где диагональная матрица ковариации шума Я Оф = 0.1е соответственно. Последовательности белых шумов в математической модели (22), (23) не зависят от начального вектора состояния этой системы х(¿0) — М(х0, П0), где Х0 = [1000м, 0 м/с, 2650 м, 150 м/с, 200 м, 0 м/с,ш0, град./с]т и П0 = diag(0.01, 0.01,0.01, 0.01, 0.01, 0.01, 0.01), а также взаимно некоррелированы.

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

Прежде всего, используем стохастический метод Эйлера с очень малым шагом дискретизации, скажем 0.0005, для решения дифференциального уравнения (22). Это позволяет определить "истинное" решение нашей модели на всем отрезке наблюдения [0, 210 с]. Затем по найденной траектории полета хехась(¿), используя уравнение наблюдателя (23), сгенерируем реализации вектора измерений ъ д через определенный временной интервал 5 = Ьд — £ д-ь С целью углубленного анализа работоспособности обсуждаемых методов нелинейной фильтрации численные эксперименты проведем для различных комбинаций угловых скоростей маневрирующего объекта (самолета) и различных значений интервала наблюдений, т. е. выбираем 8 = 2, 4, 6, 8, 10 с для каждого ш0 = 3, 4.5, 6град./с. После того как смоделированы измерения ъд на всем отрезке наблюдения через фиксированный интервал времени , решаем обратную задачу, т. е. задачу оценивания неизвестного вектора состояния стохастической модели (22) по данным наблюдателя (23). В сравнительном анализе участвуют следующие 12 численных методов, детальное описание которых представлено в приложении настоящей статьи:

• стандартный ЕКЕ и его квадратно-корневой аналог БЯ ЕКЕ (алгоритмы 1,2);

• кубатурный фильтр Калмана СБ-СКЕ (алгоритм 5) и его квадратно-корневой вариант БЯ СБ-СКЕ (алгоритм 6);

• новый сигма-точечный фильтр СБ-иКЕ и его (псевдо-) квадратно-корневая версия БЯ СБ-иКЕ (алгоритмы 7 и 8 соответственно) с тремя различными параметризациями каждый, т. е.

— СБ-иКЕ 1 — это классический ИКЕ с а = 1,0 = 0 и к = —4 (так как п = 7);

— СБ-иКЕ 2 — это ИКЕ с а = 10-3, 0 = 2, к = 0;

— СБ-иКЕ 3 — это параметризация, которая приводит к уравнениям кубатур-ного фильтра Калмана, т. е. в этом случае а =1,0 = 0, к = 0;

• параметризации квадратно-корневого алгоритма БЯ СБ-иКЕ определяются аналогичным образом;

• новый СБ-ЕКЕ и его квадратно-корневой вариант БЯ СБ-ЕКЕ (алгоритмы 9, 10).

Подчеркнем, что все эти фильтры решают задачу (22), (23) в одних и тех же условиях, т. е. их начальные значения и используемые наблюдения — одни и те же.

Итак, в результате решения задачи отслеживания маневрирующего объекта (самолета) находим оценку х^к, к = 1,... ,К, вектора его состояния для каждого исследуемого алгоритма фильтрации. Сопоставляя полученные оценки с "истинным" значением вектора состояния хехас£(Ьк), к = 1,... ,К, в одних и тех же временных точках, определяем величину ошибки оценивания. Подчеркнем, что все 12 фильтров используют одни и те же значения "истинного" решения, а также одинаково сгенерированные измерения для корректности нашего сравнительного анализа. Поскольку система (22), (23) стохастическая, проводим серии из 100 случайных экспериментов. Для каждого метода нелинейной фильтрации из перечисленных выше на основе полученного статистического материала рассчитываем накопленную среднюю квадратичную ошибку (ЛИМБЕ) оценивания на всем интервале наблюдения по следующей формуле:

ЛИМБЕ

\

1

ТК

ь к

^^^ (^ехас^к) Щ^к)

1=1 к=1 г=1

где верхний индекс г означает г-ю компоненту вектора состояния х(Ь) Е К7 системы (22), (23) (т.е. для нашей модели п = 7); I — номер эксперимента из Т = 100 случайных симуляций; — дискретный момент времени; К — общее количество точек (в которых доступны измерения) при заданном времени ожидания измерений на всем отрезке [0, 210 с] наблюдения за самолетом.

Аналогично [26] для каждого алгоритма нелинейной фильтрации дополнительно определим количество экспериментов (из проводимых 100), для которых ошибка оценивания местоположения самолета в тот или иной момент наблюдения превосходит 500 м. Другими словами, если хотя бы в один момент времени ^ взятия измерений Хк на интервале [0, 210 с] ошибка

ИМБЕр(4) = У (бехас^к) — ¿Щк) 2 + ('Пехаа(4) — Щ^) 2 + ^(ехасг(4) — (щк) > 500 м,

то счетчик отказов данного фильтра увеличивается на единицу. Общее количество отказов каждого численного метода обозначим через Р.

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

(

f й(х, г)

£ + Т£ — 12-Ш Г]

\

2

б — тшГ]--ттш б

т2 •

Г) + Т Г] + -2ш б

7] + тш б — ^ш2 <ц

с

ш

и (х, г)

0 0 0 0 0 0

0 0 0 —а1ш 0 0 —а2 г]

0 0 0 <?1 0 0 0

0 а1ш 0 0 0 0 —о2 б

0 0 0 0 0 0

0 0 0 0 0 0 0

0 0 0 0 0 0 0

2

Программы для всех численных методов, участвующих в исследовании, написаны на языке матричных вычислений МАТЬАБ. Результаты вычислительных экспериментов для стандартных реализаций алгоритмов фильтрации собраны в табл. 1 для случая ^о = 3 град./с, в табл. 2 — для = 4.5 град./с и в табл. 3 — для = 6 град./с. Обратим внимание, что соответствующие таблицы для квадратно-корневых вариантов опущены по причине их полной индентичности стандарным реализациям, за исключением результатов для сигма-точечных фильтров, которые обсуждаются в конце этого раздела.

Для каждого алгоритма нелинейной фильтрации представлена зависимость ошибки оценивания АИМБЕ и количества отказов Р от числа точек разбиения т при фиксированном значении 8 (см. соответствующие строки в каждой таблице), а также зависимость этих показателей от скорости поступления данных, т. е. протяженности времени ожидания измерений , при фиксированном т.

Таблица 1. Накопленная средняя квадратичная ошибка оценивания ЛИМБЕ и количество отказов Р из 100 случайных экспериментов, шо = 3 град./с.

5 Метод т = 8 т = 16 т = 32 т = 64 т = 128 т = 256

2 ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 1.0 ■ 103, 84

СБ-ЕКР 1.3 104, 60 4.2 ■ 102, 7 3.6 ■ 102, 2 2.6 ■102, 0 2.1 ■ 102, 0 1.9 ■ 102, 0

СБ-ШР 1 1.0 103, 100 2.5 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

ОБ-иКР 2 1.0 103, 100 2.7 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

СБ-иКР 3 те, 99 2.2 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

СБ-СКР те, 99 2.2 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

4 ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 те, 100

СБ-ЕКР 6.2 103, 66 4.3 ■ 102, 2 3.7 ■ 102, 1 2.6 ■102, 1 2.1 ■ 102, 0 2.0 ■ 102, 0

СБ-иКР 1 те, 100 2.5 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.8 ■ 102, 0

СБ-иКР 2 те, 99 2.3 ■ 102, 0 1.6 ■ 102, 0 1.8 ■102, 0 1.8 ■ 102, 0 1.8 ■ 102, 0

СБ-иКР 3 те, 98 2.4 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

СБ-СКР те, 98 2.4 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

6 ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 те, 100

СБ-ЕКР 3.0 104, 67 4.3 ■ 102, 2 3.7 ■ 102, 5 2.7 ■102, 0 2.1 ■ 102, 0 2.0 ■ 102, 0

СБ-иКР 1 1.0 103, 100 2.5 ■ 102, 0 1.7 ■ 102, 0 1.8 ■102, 0 1.8 ■ 102, 0 1.8 ■ 102, 0

СБ-иКР 2 те, 99 2.3 ■ 102, 0 1.6 ■ 102, 0 1.8 ■102, 0 1.8 ■ 102, 0 1.8 ■ 102, 0

СБ-иКР 3 те, 99 2.4 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

СБ-СКР те, 99 2.4 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

8 ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 1.0 ■ 103, 81

СБ-ЕКР 1.1 104, 72 4.3 ■ 102, 8 3.7 ■ 102, 1 2.7 ■102, 0 2.1 ■ 102, 0 2.0 ■ 102, 0

СБ-иКР 1 1.0 103, 100 2.3 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

СБ-иКР 2 те, 100 2.3 ■ 102, 0 1.6 ■ 102, 0 1.8 ■102, 0 1.8 ■ 102, 0 1.8 ■ 102,

СБ-иКР 3 те, 100 2.3 ■ 102, 0 1.6 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

СБ-СКР те, 100 2.3 ■ 102, 0 1.6 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

10 ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 1.0 ■ 103, 87

СБ-ЕКР 6.1 103, 61 4.2 ■ 102, 4 3.6 ■ 102, 2 2.7 ■102, 1 2.2 ■ 102, 0 2.1 ■ 102, 0

СБ-иКР 1 1.5 104, 100 2.7 ■ 102, 0 1.7 ■ 102, 0 1.7 ■102, 0 1.7 ■ 102, 0 1.7 ■ 102, 0

СБ-иКР 2 те, 100 2.3 ■ 102, 0 1.6 ■ 102, 0 1.8 ■102, 0 1.8 ■ 102, 0 1.8 ■ 102, 0

СБ-иКР 3 те, 98 2.3 ■ 102, 0 1.6 ■ 102, 0 1.8 ■102, 0 1.8 ■ 102, 0 1.8 ■ 102, 0

СБ-СКР те, 98 2.3 ■ 102, 0 1.6 ■ 102, 0 1.8 ■102, 0 1.8 ■ 102, 0 1.8 ■ 102, 0

Для упрощения анализа введем следующие обозначения. Если ЛИМБЕ > 105, то обозначим такие ошибки через то. Если в любой момент времени на интервале наблюдения за самолетом произошел аварийный сбой того или иного алгоритма фильтрации в силу невозможности выполнить разложение Холецкого матриц ковариации (или малоранговую модификацию квадратных корней матрицы ковариации), то обозначим в таблицах такие аварийные ситуации прочерком "—".

Проанализируем данные, представленные в табл. 1-3, и прежде всего обратимся к результатам работы кубатурного фильтра Калмана. Так, для строк с маркером СБ-СКЕ можно сделать следующий вывод. В целом результаты наших экспериментов подтверждают выводы работы [26] в части, относящейся к кубатурному фильтру Калмана. Например, для ш0 = 3 град./с СБ-СКЕ дает 0% отказов из 100 проведенных симуляций (за исключением случая т = 8). Однако с увеличением задача слежения за самоле-

Таблица 2. Накопленная средняя квадратичная ошибка оценивания ЛИМБЕ и количество отказов Р из 100 случайных экспериментов, шо = 4.5 град./с.

5 Метод т = 8 т = 16 т = 32 т = 64 т = 128 т = 256

2 ЕКР то, 100 то, 100 то, 100 то, 100 то, 100 то, 100

СБ-ЕКР то, 100 1.1 103, 98 8.2 102 17 7.1 ■ 102, 7 6.3 ■ 102, 1 5.7 ■ 102, 0

СБ-ШР 1 то, 100 то, 92 4.8 102 0 3.9 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

ОБ-иКР 2 то, 100 1.3 103, 92 4.9 102 0 3.9 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

СБ-иКР 3 то, 100 то, 53 4.7 102 0 3.8 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

СБ-СКР то, 100 то, 53 4.7 102 0 3.8 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

4 ЕКР то, 100 то, 100 то, 100 то, 100 то, 100 то, 100

СБ-ЕКР то, 100 1.1 103, 93 8.2 102 14 7.4 ■ 102, 9 6.6 ■ 102, 3 5.8 ■ 102, 0

СБ-иКР 1 то, 100 то, 94 4.8 102 0 3.9 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

СБ-иКР 2 — то, 94 4.8 102 0 3.9 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

СБ-иКР 3 то, 100 то, 57 4.7 102 0 3.9 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

СБ-СКР то, 100 то, 57 4.7 102 0 3.9 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

6 ЕКР то, 100 то, 100 то, 100 то, 100 то, 100 то, 100

СБ-ЕКР то, 100 1.1 103, 97 8.2 102 20 7.4 ■ 102, 14 6.6 ■ 102, 4 5.9 ■ 102, 0

СБ-иКР 1 то, 100 то, 90 4.9 102 0 3.9 ■ 102, 0 3.8 ■ 102, 0 3.8 ■ 102, 0

СБ-иКР 2 — 1.3 103, 90 4.9 102 0 3.9 ■ 102, 0 3.8 ■ 102, 0 3.7 ■ 102, 0

СБ-иКР 3 то, 100 то, 47 4.9 102 0 3.9 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

СБ-СКР то, 100 то, 47 4.9 102 0 3.9 ■ 102, 0 3.7 ■ 102, 0 3.7 ■ 102, 0

8 ЕКР то, 100 то, 100 то, 100 то, 100 то, 100 то, 100

СБ-ЕКР то, 100 1.1 103, 94 8.3 103 95 8.3 ■ 103, 93 6.6 ■ 102, 4 5.7 ■ 102, 0

СБ-иКР 1 то, 100 то, 91 то 97 то, 90 3.9 ■ 102, 0 3.2 ■ 102, 0

СБ-иКР 2 — то, 91 то 97 то, 92 3.9 ■ 102, 0 3.2 ■ 102, 1

СБ-иКР 3 то, 100 то, 58 то 97 то, 97 3.9 ■ 102, 0 3.2 ■ 102, 1

СБ-СКР то, 100 то, 58 то 97 то, 97 3.9 ■ 102, 0 3.2 ■ 102, 1

10 ЕКР то, 100 то, 100 то, 100 то, 100 то, 100 то, 100

СБ-ЕКР то, 100 1.1 103, 94 8.3 103 97 8.3 ■ 103, 98 6.5 ■ 102, 2 5.7 ■ 102, 0

СБ-иКР 1 то, 100 то, 93 то 95 то, 91 3.9 ■ 102, 0 3.3 ■ 102, 0

СБ-иКР 2 — то, 93 то 95 то, 98 3.9 ■ 102, 0 3.3 ■ 102, 0

СБ-иКР 3 то, 100 то, 97 то 95 то, 95 3.8 ■ 102, 0 3.3 ■ 102, 0

СБ-СКР то, 100 то, 97 то 95 то, 95 3.8 ■ 102, 0 3.3 ■ 102, 0

том, выполняющим поворот, существенно усложняется. Так, уже при = 4.5 град./с и том же самом количестве точек дополнительного разбиения, т. е. при т =16, которые ранее были достаточны для нормальной работы фильтра, теперь мы наблюдаем 100 %-ный отказ (см. табл. 2). Понятно, что количество точек разбиения т необходимо увеличить до 32, чтобы СВ-СКГ снова отслеживал маневрирующий объект с приемлемой точностью. С другой стороны, если мы увеличиваем период ожидания новой информации до 8 = 8 си более, то ошибка дискретизации снова начинает существенно влиять на точность фильтра и приводит к 100 %-ному показателю отказов. При ш0 = 6 град./с наблюдается схожее поведение в работе СВ-СКГ с той лишь разницей, что задача слежения становится еще более сложной из-за увеличения скорости выполнения поворота летательным аппаратом. Из табл. 3 следует, что при небольших временах ожидания можно ограничиться сеткой с т = 32 дополнительными точками, так как отказы

Таблица 3. Накопленная средняя квадратичная ошибка оценивания ЛИМБЕ и количество отказов ^ из 100 случайных экспериментов, ^о = 6 град./с.

5 Метод т = 8 т = 16 т = 32 т = 64 т = 128 т = 256

2 ЕКР СБ-ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 1.3 ■ 103, 95 те, 100 8.2 ■ 102, 4 те, 100 5.9 ■ 102, 0 те, 100 7.6 ■ 102, 0

СБ-иКР 1 ОБ-иКР 2 СБ-иКР 3 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 4.8 ■ 102, 0 4.9 ■ 102, 0 6.3 ■ 102, 1 3.9 ■ 102, 0 3.9 ■ 102, 0 3.9 ■ 102, 0 4.6 ■ 102, 0 4.6 ■ 102, 0 4.6 ■ 102, 0 4.7 ■ 102, 0 4.7 ■ 102, 0 4.7 ■ 102, 0

СБ-СКР те, 100 те, 100 6.3 ■ 102, 1 3.9 ■ 102, 0 4.6 ■ 102, 0 4.7 ■ 102, 0

4 ЕКР СБ-ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 1.3 ■ 103, 93 те, 100 8.5 ■ 102, 4 те, 100 6.0 ■ 102, 1 те, 100 7.5 ■ 102, 1

СБ-иКР 1 СБ-иКР 2 СБ-иКР 3 те, 100 те, 100 те, 100 те, 100 те, 100 4.8 ■ 102, 0 4.8 ■ 102, 0 5.9 ■ 102, 0 3.9 ■ 102, 0 3.9 ■ 102, 0 3.9 ■ 102, 0 4.6 ■ 102, 0 4.6 ■ 102, 0 4.6 ■ 102, 0 4.6 ■ 102, 0 4.6 ■ 102, 0 4.7 ■ 102, 0

СБ-СКР те, 100 те, 100 5.9 ■ 102, 0 3.9 ■ 102, 0 4.6 ■ 102, 0 4.7 ■ 102, 0

6 ЕКР СБ-ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 1.3 ■ 103, 91 те, 100 те, 100 те, 100 7.9 ■ 102, 0 те, 100 7.7 ■ 102, 0

СБ-иКР 1 СБ-иКР 2 СБ-иКР 3 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 98 те, 91 те, 100 7.1 ■ 102, 0 7.1 ■ 102, 0 7.1 ■ 102, 0 5.5 ■ 102, 0 5.5 ■ 102, 0 5.5 ■ 102, 0

СБ-СКР те, 100 те, 100 те, 100 те, 100 7.1 ■ 102, 0 5.5 ■ 102, 0

8 ЕКР СБ-ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 1.3 ■ 103, 96 те, 100 те, 100 те, 100 те, 100 те, 100 7.7 ■ 102, 2

СБ-иКР 1 СБ-иКР 2 СБ-иКР 3 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 90 те, 100 2.5 ■ 104, 14 1.0 ■ 103, 16 1.1 ■ 103, 14 5.8 ■ 102, 0 5.8 ■ 102, 1 5.8 ■ 102, 2

СБ-СКР те, 100 те, 100 те, 100 те, 100 1.1 ■ 103, 14 5.8 ■ 102, 2

10 ЕКР СБ-ЕКР те, 100 те, 100 те, 100 те, 100 те, 100 1.3 ■ 103, 98 те, 100 те, 100 те, 100 те, 100 те, 100 7.6 ■ 102, 1

СБ-иКР 1 СБ-иКР 2 СБ-иКР 3 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 100 те, 91 те, 100 те, 78 те, 65 те, 87 6.7 ■ 102, 1 6.7 ■ 102, 1 6.6 ■ 102, 1

СБ-СКР те, 100 те, 100 те, 100 те, 100 те, 87 6.6 ■ 102, 1

составляют всего лишь 1 %. Однако при уменьшении скорости поступления данных измерений, т. е. при увеличении , становится очевидным, что необходимо увеличить количество точек разбиения до т = 128, а чтобы обеспечить стабильное функционирование кубатурного фильтра, т. е. приемлемую точность вычислений и гарантированно малое количество отказов для всевозможных комбинаций угловой скорости и длины интервала наблюдения, смоделированных в наших экспериментах, необходимо применить CD-CKЕ с т = 256 на каждой итерации этого метода.

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

В заключение отметим, что кубатурный фильтр, предложенный [26], не является адаптивным. Другими словами, он недостаточно гибок и, самое главное, не самонастраивается на практическую задачу. Поэтому CD-CKЕ требует ручной подстройки под контретную модель. Это предполагает дополнительные затраты на такое предварительное исследование, чтобы определить оптимальное количество точек разбиения т, необходимое для надежной и точной работы данного алгоритма фильтрации. Однако в случае подобной настройки CD-CKЕ обеспечивает высокое качество оценивания вектора состояния стохастической дифференциальной системы.

Обратимся теперь к результатам работы нового сигма-точечного фильтра Калмана. В табл. 1-3 им соответствуют строки с маркерами CD-UKЕ 1, CD-UKЕ 2 и CD-UKЕ 3. Анализируя полученные данные, можно сделать следующий вывод. Прежде всего реализация CD-UKЕ 3 действительно эквивалентна уравнениям кубатурного фильтра Калмана. Накопленная средняя квадратичная ошибка оценивания (ЛИМБЕ) и количество отказов ( Р) алгоритмов CD-UKЕ 3 и CD-CKЕ одинаковы для всех серий экспериментов. Однако кубатурный фильтр подразумевает наличие эквивалентной квадратно-корневой реализации, что существенно для практического применения таких методов, так как удешевляет вычисления и повышает их устойчивость по отношению к ошибкам округления. Этот вопрос подробно обсуждался в разд. 2.1 и 2.2. Напомним, что стандартные реализации сигма-точечного и кубатурного фильтров Калмана требуют осуществления разложения Холецкого матриц ковариации фильтра на каждом этапе алгоритма. К сожалению, подобная операция иногда невыполнима в силу влияния ошибок округления и/или дискретизации. Тогда происходит аварийная остановка работы. Этого можно избежать за счет перехода к эквивалентной квадратно-корневой версии соответствующего метода. Для кубатурного фильтра Калмана данная задача решена и эквивалентная квадратно-корневая реализация существует (см. алгоритм 6), а вот для сигма-точечного фильтра таких эквивалентных аналогов нет (см. подробное разъяснение этого вопроса в разд. 2.1). Другими словами, сигма-точечные алгоритмы Калмана являются более гибкими и общими для решения задачи нелинейной фильтрации, но они менее стабильны, поскольку подвержены аварийным остановкам и сбоям в работе. В частности, этот эффект наблюдается и в проведенных нами экспериментах. В табл. 2 и 3 этому соответствуют строки с прочерком "—". Например, они особенно характерны для CD-UKЕ 2.

Таким образом, согласно нашим наблюдениям, сигма-точечный фильтр с параметризацией а = 10-3, 0 = 2, к = 0 оказался наименее удачной реализацией среди рассмотренных в настоящей статье и привел к наиболее частым сбоям в работе из-за невозможности выполнить разложение Холецкого приближенной матрицы ковариации в стандартном алгоритме СБ-иКЕ 2 (или малоранговую модификацию ее квадратного корня в (псевдо-)квадратно-корневой версии БЯ СБ-иКЕ 2). В остальном результаты экспериментов для сигма-точечного фильтра Калмана повторяют результаты кубатурного фильтра и приводят к тем же самым выводам, что уже сделаны выше. Однако нельзя не отметить тот факт, что новый сигма-точечный СВ-ИКЕ существенно превосходит ИКЕ-вариант [26], который давал большую ошибку оценивания (ЛИМБЕ) и 90-100% отказов. Наши эксперименты говорят о том, что новый СВ-ИКЕ в целом повторяет поведение кубатурного фильтра Калмана, т. е. дает ту же ошибку оценивания и то же количество отказов.

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

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

Третий важный вывод касается ЕКЕ. Если обратиться к результатам, представленным в табл. 1-3, то нетрудно видеть, что классический ЕКЕ не справляется ни с одной из рассмотренных задач (см. строки с маркером ЕКЕ). Даже при невысокой начальной угловой скорости ш0 и большом количестве точек разбиения т на каждом интерва-

ле ожидания новой информации он приводит к слишком большой ошибке оценивания и 100% отказов. То же самое подтверждается и результатами экспериментов в [26]. Однако новый алгоритм (построенный на разложении Ито — Тейлора порядка 1.5 для дискретизации стохастического дифференциального уравнения), т. е. СВ-ЕКР, неплохо справляется с поставленной задачей. Он существенно превосходит стандартный ЕКР, основанный на дискретизации стохастическим методом Эйлера, но все же уступает современным техникам, таким как сигма-точечный и кубатурный фильтры Калмана.

Это очень интересный результат, поскольку он свидетельствует о том, что именно ошибка дискретизации стохастического дифференциального уравнения модели оказывает критическое влияние на работу любого нелинейного фильтра. С другой стороны, так как все наши методы тестировались в одинаковых условиях, т. е. с одной и той же схемой дискретизации, разница в качестве фильтрации современных техник и СВЕКР — это следствие влияния второй ошибки, т. е. ошибки аппроксимации моментов. Из полученных данных видно, что влияние этой ошибки не слишком велико. Новый СВ-ЕКР лишь немного уступает современным методам при ш0 = 3 град./с. В целом ему требуется большее количество точек разбиения т на каждом интервале ожидания , чтобы обеспечить высокую точность оценивания и 0%% отказов. При увеличении ш0 до значения 6 град./с необходимое для надежной работы количество точек т существенно возрастает. Но если это условие выполнено, то его накопленная ошибка оценивания лишь немногим превосходит аналогичный показатель у современных методов нелинейной фильтрации, которые обеспечивают теоретически более высокий порядок аппроксимации моментов нормального распределения. То же самое справедливо и в отношении количества отказов.

Таким образом, несмотря на частую критику ЕКР в зарубежной и отечественной литературе, эта техника заслуживает более внимательного отношения. Следует понимать, что под ЕКР обычно подразумевают только алгоритм 1, основанный на стохастическом методе Эйлера. На самом деле ЕКР — это не единственный метод, а целое направление, базирующееся на определенных принципах построения фильтров данного класса (см. также [38]). Как показывают представленные здесь результаты экспериментов, в рамках этого класса можно строить очень интересные численные методы, эффективные для широкого спектра прикладных задач. Однако определенным недостатком ЕКР по сравнению с более современными фильтрами является необходимость вычисления матрицы Якоби нелинейной функции стохастической системы. В значительной мере этот недостаток компенсируется разложением Холецкого матрицы кова-риации, являющимся неотъемлемой частью стандартных иКР и СКР, которое к тому же чувствительно по отношению к ошибкам дискретизации и округления. Кроме того, методы ИКР и СКР, которые не требуют вычисления матрицы Якоби при оценивании дискретных систем, полностью утрачивают это преимущество в случае применения к стохастическим дифференциальным уравнениям из-за необходимости вычисления дифференциальных операторов Ь0 и Ь, заданных формулами (15) и (16), на каждой итерации СВ-СКР и СВ-ИКР (см. алгоритмы 5 и 7 в приложении статьи). Дополнительно отметим, что для более общих стохастических дифференциальных уравнений вида (1), в которых матрица диффузии не является постоянной, формулы для вычисления операторов Ь0 и Ь приобретают еще более сложный вид (см., например, [39, разд. 10.4]).

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

и стандартных реализаций, работающих с полными матрицами ковариации, в части, относящейся к EKF и CKF, т.е. результаты оценивания для квадратно-корневых реализаций EKF и CKF идентичны результатам их стандартных реализаций, собранным в табл. 1-3. С другой стороны, для сигма-точечного фильтра результаты его квадратно-корневой реализации немного отличны от результатов стандартной версии UKF. Как говорилось ранее, это следствие невозможности построить для UKF эквивалентную квадратно-корневую реализацию. Все подобные методы, встречаемые в литературе, являются "псевдо" квадратно-корневыми. Наши эксперименты показывают, что в целом они работают не лучше оригинальных версий.

Заключение

Построены новые сигма-точечный и обобщенный фильтры Калмана для оценивания неизвестного вектора состояния стохастической дифференциальной системы с дискретными измерениями на основе разложения Ито — Тейлора порядка 1.5, примененного для дискретизации исходного стохастического дифференциального уравнения. Проведен сравнительный анализ новых методов с уже известным кубатурным фильтром Кал-мана, разработанным на основе той же самой дискретизации порядка 1. 5. Все алгоритмы нелинейной фильтрации протестированы на задаче отслеживания координат и скоростей самолета, осуществляющего маневр в горизонтальной плоскости. Показано, что при корректной (т. е. одинаковой) дискретизации современные UKF и CKF обеспечивают практически одну и ту же точность оценивания неизвестного вектора состояния стохастической дифференциальной системы. Этот вывод полностью соответствует теоретическим результатам, полученным ранее в области нелинейной фильтрации.

Результаты экспериментов говорят также о том, что при построении EKF-метода со схемой дискретизации того же порядка, что и в CD-UKF и CD-CKF, новая реализация CD-EKF существенно превосходит стандартную и не сильно уступает современным методам в точности оценивания. Дополнительно проанализированы особенности практической реализации современных нелинейных алгоритмов калмановской фильтрации, а также исследована их работоспособность для различных комбинаций угловых скоростей маневрирующего объекта (самолета) и различных значений интервала ожидания наблюдений. Показано, что все они могут применяться для качественного определения местоположения цели, если ошибка дискретизации соответствующего стохастического дифференциального уравнения не оказывает существенного влияния на точность фильтрации.

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

Приложение

Алгоритм 1 (ЕКР).

0. Априорные данные (момент к = 0).

• Начальные значения: х0|0 = хо, Ро|о = По-

1. Этап экстраполяции (к = 1 ,...,К). В момент времени Ьк-\ известны Хк-1\к-1 и Рк-1\к-1- На временном интервале [¿/с-ъ^] предсказать щк_1 и Рк\к_х (5 = Ьк - ^-1):

• оценка вектора состояния:

*к\к-1 = *к-1\к-1 + ^ {*к-1\к-1^к-1)]

• ковариация ошибки предсказания:

Рк\к-г = (1п + ОД Р^.г {1п + +

где Рк = д( (хк_11к_1,Ьк-1) /.

1. Этап обработки измерений и фильтрации (к = 1 ,...,К).

В момент времени известны предсказанные значения ~х.к\к-\ и Рк\к-\. Вычислить скорректированные оценки хцк и Рк\к следующим образом:

• невязка измерений: ек = гк — Ь(хд.|д._1);

• ковариация невязки измерений: Ке^к = К + НкРщк_1Нк;

• коэффициент обратной связи: Кк ^ = Рцк-гЩ^ёЬ

• оценка вектора состояния: хцк = хцк_\ + KkJek]

• ковариация ошибки оценивания:

Рк\к = Рк\к-1 ~ Кк,/НкРк\к_ъ

где Нк = сШ (хк\к-1) /сЬс(£).

оо оо

Алгоритм 2 (БИ ЕКР).

0. Априорные данные (момент к = 0).

• Разложение15: П0 = П^Иц72, К = П1/2ПТ/2.

• Начальные значения: х0|0 = хо, Р^ = П^2.

1. Этап экстраполяции (к = 1 ,...,К). В момент времени

1/2

известны и Рк_цк_1- На временном интервале [1к-\,1к]

1/2

предсказать ±к\к_г и (5 = -

• оценка вектора состояния:

Х-к\к-1 = *-к-1\к-1 + ^ {*-к-1\к-1^к-1)]

• квадратный корень матрицы ковариации ошибки предска-

зания: Р.

1/2

к\к-1

(1п + ОД рЦ\к_х ^ЬС \ 6,

где Рк = (Я (хк_1\к_1, /и в — матрица ортогонального преобразования, приводящего правую часть этой формулы к нижнему треугольному виду. 2. Этап обработки измерений и фильтрации (к = 1 ,...,К). В момент времени ¿д. известны предсказанные значения ~х.к\к-\

и Рк\к-г Получить оценки ±к\к и Р^к:

• найти Рцк , Kfík и В^к с помощью

Ке,к 0 'в1/2

К^к р1/2 к\к . 0

1кРк{к_1 р1/2 гк\к-1

в,

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

~ Т —1/2

матрице, указанной в левой части, а К^к = Рк\к-\Нк к >

нк = (Пл (хА.|А._1) /сЬ<£).

► невязка измерений: ек = гк — Ь(хд.|д._1);

Л Л - _т/2

► оценка вектора состояния: хк\к = ъ.к\к-\ + KfíkRe к ек.

Ьа |

1

й

!

I Й

Стандартный иКР

Алгоритм 3 (икр).

0. Априорные данные (к = 0).

• Начальные значения: х0|0 = хо, Ро|о = По-

1. Этап экстраполяции (к = 1 ,...,К). В момент времени Ьк-\ известны хк_1\к_1 и Рк_цк_1- На временном интервале предсказать щк_1 и Рк\к-1 (5 = Ьк - ^-1):

разложение15: Рк_цк_\

р1/2 рТ/2

к—1\к—1 к-1\к-У вычислить сигма-точки19 (для 7 = л/п + Л)

*

Д' X

0,к-1\к-\ — хк-1\к-1>

_ - I тз1/2

г,к-1\к-1 - хк-1\к-1

_ - тз1/2

г,к-1\к-1 - Х.к-1\к-1~ 1и,к-1\к-Г

• наити прогнозные значения сигма-точек:

^*,к-1\к-1 = ^ {^г,к-1\к-1^к-\) , г = 0,... ,2щ

• прогнозная оценка вектора состояния:

2та (т) г=0 1

• ковариация ошибки предсказания:

2га с ■) /

г Л**

Я

/с | Лс— 1

=

г=0

i,k-l\k-l

— X

/с | /с—1 т

Квадратно-корневой иКР

Алгоритм 4 (БИ ШР).

0. Априорные данные (к = 0).

• Разложение15: П0 = П^Иц72, К = П1/2ПТ/2.

• Начальные значения: х0|0 = хо, Рщ^ = П^2.

1. Этап экстраполяции (к = 1 ,...,К). В момент времени

1/2

известны ~кк_цк_1 и Рк_цк_1- На временном интервале [1к-\,1к]

1/2

предсказать х.к\к-1 и Рк\к-\ = ^ ~ tk_1):

• вычислить: Х^к-\\к-\ из алгоритма 3;

• найти прогнозные значения сигма-точек:

^*,к-1\к-1 = ^ {^г,к-1\к-11 , г = 0,..., 2щ

• прогнозная оценка вектора состояния:

2 га , ,

*к\к-1 = Е и^лг?

г=0

' г,Аг— 1|Лг— 1'

• квадратный корень матрицы ковариации ошибки предска-,1/2 _

зания: Р.

к\к-1

ИЛ1(с)Х:

:к-1\к-1

6, где в

мат-

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

а Х*_1|А._1 -ях 2п-матрица вида

\-l\k-l

обновить Рк(к_1 с учетом :

У*

^г,к-1\к-1

Р,

1/2

к\к

хк\к-1> (с).

г = 1,..., 2щ

к\к-

1 = с1ю1ирс!аге {р1(к_г " *к\к-ъ

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

19 Здесь и далее первый нижний индекс означает порядковый номер столбца в соответствующей матрице Хк-1\к-1 или

к—1\к — 1'

Стандартный иКР (продолжение)

2. Этап обработки измерений и фильтрации (к = 1 ,...,К).

В момент времени ¿д. известны ^-Цк-х и Рк\к-1- Получить оценки хцк и Рк\к следующим образом:

• разложение15: Рк\к-1 = Рк\к-1Рк\к-г

• вычислить сигма-точки19 (для 7 = л/п + Л)

Хо,к\к-1 = ^к\к-11

Хг,к\к-1 = +1Р1,к\к-Г

•V _ - о!/2

• найти прогнозные значения: %Ч,к\к-1 = Ь (Хг,к\к-1) ,г = 0,... ,2щ

2 п

• ВЫЧИСЛИТЬ гк\к-1 = Е ^т)ъг,к\к-ъ

г=0

• оценка ковариации невязки измерений:

л (с) т

г=0

• оценка кросс-ковариации:

л (с) т

г=О

• оценка коэффициента обратной связи:

Кк,! =

• оценка вектора состояния:

Xк\к = *к\к-1 + {%к ~ Щ\к-\)',

• ковариация ошибки оценивания:

рк\к = Рк\к-1 ~ Кк>{Ке>кК1г

Квадратно-корневой иКР (продолжение)

2. Этап обработки измерений и фильтрации (к = 1 ,...,К).

/ч 1/2

В момент времени ¿д. известны ~х.к\к-\ и Получить оцен-

ки х.к\к и Рцк следующим образом:

• вычислить: Х^щк-1 из алгоритма 3;

2 п

• найти значения: Ъ^к\к-1

г(т),

• вычислить: %к\к-1 = Е

г=0

• ковариация невязки измерений

г,к\к—11

р1/2 _

к\к-1

я1/2

е,

где в — матрица ортогонального преобразования, приводящего правую часть этой формулы к нижнему треугольному виду, а %к\к-1 — п х 2п-матрица вида %к\к-1 = [• • • > ^Ч,к\к-1 — Щ\к-ъ • • •] > г = 1, . . . ,2щ

л 1/2

обновить Я , с учетом W^

{с)

О '

т. е.

Ц1Л = с1ю1ирс1аге { Я1'' Ъ

>1/2

1ек > ^0,к\к-1~ '¿к\к-Ъ И^о

(с)

• оценка кросс-ковариации:

2п ( 1

Рх,г = Е Щ {Хг,к\к-1 -Щк-1)

г=0

{2ч,к\к-1 ~ ЪЩк-1) • оценка коэффициента обратной связи:

кк,г = [рх,л~е1/2) КТ'1

• оценка вектора состояния: Ик\к = %к\к-1+Кк,/ (як ~ ^Цк-х)'-,

Л 1/2

• вычислить: II = Кк,/Яе к >

• квадратный корень матрицы ковариации ошибки оценива-

ния^: = с1ю1ирс1аге \ и, -1

к\к

,1/2

к\к-

203аметим, что так как II — матрица, то малоранговая модификация сЬо1ирс^е квадратного корня Рк{к_± выполняется последовательно, т.е. цикле, для каждого столбца матрицы V.

Стандартный СБ-СЕТ

Алгоритм 5 (СБ-СКР).

0. Априорные данные (к = 0).

• Начальные значения: х0|0 = хо, Ро|о = По-

1. Этап экстраполяции (к = 1 ,...,К). В момент времени Ьк-\ известны хк_цк_\ и Рк_\\k~\- На временном интервале [¿¿¡-ь^] предсказать и Рк\к-1 следующим образом (5 = Ьк — tк-l):

• X

(0)

к-1\к-1 ~ Хк-1\к-1, Рк1цк_1 — ^к-1\к-1-• Выполнить т-шаговую процедуру обновления оценок, т = 5/т,] = 0, т — 1:

(о)

Рь

1'

осуществить разложение СО _ fpU) \1/2

15.

Р,

Р,

(з)

Т/2

к-1\к-1 к-1\к-1] \±к-1\к-1/ 2* найти21 кубатурные точки ^ из (13) и >(з) _ Аз) , (г>(з) ^1/2

Д'

— 4-

-1\к-1— ^к-1\к-1 * \г к-1\к-1)

3* прогнозные значения кубатурных узлов:

г,к

X

- г

fc-l|fc-l id I t,fc-l|fc-l

M^il-^fc-i+jr);

4* оценка состояния:

2n,tl i.fc-1lfc"1'

5* ковариация ошибки предсказания:

0-+1) _ чг*.(Я-1) , гГГ?4-

/с—1|/с— 1 - Жк-1\к-1{Жк-1\к-1) +Т+

р,

+ +

—I f' 3 ^fc-ilfc-i

1-2 г ' f(j)

г

У

G Lf

Lfc-i|fc-i . Т

fc-llfc-l

+ Lf

U) k-l\k-l

GT

где Lf^ =Lf [^'vtk.1+jr

1| к

•M

-1| k-

квадратно-корневой сб-скр

Алгоритм 6 (БИ СБ-СКР).

0. Априорные данные (к = 0).

• Разложение15: П0 = П^Пд72, К = П1/2ПТ/2.

• Начальные значения: х0|0 = хо, Рщ^ = П^2.

1. Этап экстраполяции (к = 1 ,...,К). В момент времени

1/2

известны ~кк_цк_1 и Рк_цк_1- На временном интервале [1к-\,1к]

1/2

предсказать ±к\к_г и Р^к_х (5 = -

¡.(о)

* Хк-1\к-1 ~ Хк-1\к-1> \Pje- 1\к-1) ~ * к-1\к-Г

• Выполнить т-шаговую процедуру обновления оценок.

(0)

V/2_pl/2

г = 5/т, ] = 0, т — 1: 1* найти21 кубатурные точки & из (13) и

X

(з)

— х^ + I РКЛ ) ' Р ■ прогнозные значения кубатурных узлов:

,(з)

1/2

X

+ - г

fc-l|fc-l ld \^i,k-l\k-l

(3+1)

1 2 n

3- оценка состояния: ^¿^ = —E

2ni=i

4* квадратный корень ковариации ошибки предсказания: ,(з+1) \1/2_

Р,

к-1\к-1

где Lfi-i|fc-i = Lf {t-k-nk-vtk-i + зг

см. продолжение реализаций кубатурного фильтра Калмана...

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

(о) fc-i|fc-i

Р,

(о) fc-llfc-l

1/2

Стандартный СБ-СЕТ (продолжение)

* (?' I 1)

^к-1\к-1 ~ п х 2п-матрица вида

,0+1) _ 1

к~1\к~1 ^

• определить прогнозные значения:

_ ~(то) р _ р(то)

2. Этап обработки измерений и фильтрации (к = 1 ,...,К).

В момент времени Ьк известны и Получить оцен-

ки хцк и следующим образом:

15 г> т-, 1/2 пТ/2

• разложение15: Р^ =

• вычислить кубатуриые точки ^ из (13) и

1/2

(Л'г)^-! = *-к\к-1+ Рк\к_1$г, 1 = 1,...,2 Щ

• прогнозные значения кубатурных узлов: %ч,к\к-1 = Ь (Х^к\к_]) , г = 1,..., 2щ

2 п

1 \ - г,

• вычислить: ък\к_х =

г=1

• ковариации невязки измерений: Яе>к = + К, где Ък\к-1 — п х 2п-матрица вида

1 г

^к\к-1 = • • • > С^г)к\к-1 - Щк-Ъ ■ ■

• оценка кросс-ковариации: Рх>х =

где ^к\к-\ — п х 2п-матрица вида 1

^к\к-1 = -/== [• • • > Хг,к\к-1 ~ ^к\к-Ь • • •

V 2и

• коэффициент обратной связи: Кк ^ = Рх,гЯёЬ

• оценка вектора состояния:

*-к\к = *-к\к-1 + Кк,! {%к ~ %к\к-1)',

• ковариация ошибки оценивания:

Рщк = Рк\к-1 -

Квадратно-корневой СБ-СКР (продолжение)

в — ортогональное преобразование, приводящее правую

ф , - р1/2 - (

Р

часть к нижней треугольной матрице

• определить прогнозные значения:

ы V72 к-1\к-1) '

2. Этап обработки измерений и фильтрации (к = 1

лучить оценки хд,|к и Рщ следующим образом:

• вычислить кубатурные точки & из (13) и

1/2

(Хг)к\к-1= *-к\к-1+ « = 1> • • • > 2п;

• прогнозные значения кубатурных узлов:

1/2

,К). По-

Ч,к\к-1

Ь {Х^к\к-1) , г = 1,

,2п;

1

2 га

• вычислить:

Е2*

%=\ л 1/2 ± ^

• определить матрицы Ке к , Рк[/ и из

Я

1/2

е.к

Рт

р,

е,

1/2 к\к

к\к-1 Р^^2

■к\к-1 О

где В — ортогональное преобразование, приводящее к нижнему треугольному виду матрицу в правой части этой формулы, а "^цк-ъ ^к\к-1 — п х 2п-матрицы вида 1

О

1/2 к\к

Z

к\к-1

Щк-1

у/2п 1

у/2п

1,к\к-1 ~ ЪЩк-!-,

г,к\к-1 ~ хк|к-1

?-1/2

коэффициент обратной связи: = Рх,хРек

— л. л. _гр/2

(Рх,г = Рх,гРе>к В алгоритме 4);

вектор состояния: хк\к = хк\к_1 + Кк>; (ък - ъщк-/)-

Реализации нового сигма-точечного фильтра Калмана

(CD-UKF)

Стандартный CD-UKF

Алгоритм 7 (CD-UKF).

0. Априорные данные (к = 0).

• Начальные значения: х0|0 = хо, Ро|о = По-

1. Этап экстраполяции (к = 1 ,...,К). В момент времени tk~i известны хк_цк_\ и Рк_\\k~\- На временном интервале [tk~i,tk] предсказать и Рк\к-1 следующим образом (5 = tk—tk_1):

• xfc-l|fc-l - xfc-l|fc-l, ^fc-llfc-l - Рк-1\к-1-

• Выполнить m-шаговую процедуру обновления оценок, т = 5/m, j = 0, т — 1:

1* осуществить разложение15:

Р,

СО

Р

СО

U) f/2.

к-1\к-1 к-Цк-lJ \гк-1\к-1) ' 2* вычислить сигма-точки (7 = л/п + Л)

Д' Д'

СО

0,fc-l|fc-l СО

i,fc-l|fc-l

X

X

СО fc-llfc-l>

CO

>co

-llfc-l ^ ' ^ t,fc-llfc-l^

ч 1/2

Л'

- X(i) - 'Y I PKJ> \

-llfc-l Afc-llfc-l ' ^ t,fc-llfc-l j

3* найти прогнозные значения сигма-точек:

v*,0'+i) —ff у(Л + I .

^i,fc-i|fc-i — V^ i,fc—i|fc—1 ''•fc-1 JT)■>

2n

4« вектор состояния: 4-+i|Li = £

i=0

5* ковариация ошибки предсказания:

2 n

>C0

ч 1/2

Р,

Ü+1)

fc-l|fc-l

= E^,

i=0

(с) fд,*,С? + 1)

- xü+1) i,fc-l|fc-l fc-l|fc-l

>(J+1)

T

Квадратно-корневой CD-UKF

Алгоритм 8 (SR CD-UKF). 0. Априорные данные (к = 0).

• Разложение15: П0 = П^Иц72, R = Rl/2RT/2.

• Начальные значения: x0|0 = хо, Рс

1/2 0|0

TL

1/2

1. Этап экстраполяции (к = 1 ,...,К). В момент времени 1к-\

1/2

известны ~кк_цк_1 и Рк_цк_1- На временном интервале

1/2

предсказать и Р^^ (5 = гк-

Хо)

* xfc-i|fc-i~ ®fc-i|fc-i> (^i-iifc-i) • Выполнить m-шаговую процедуру обновления оценок.

Л V V pl/2

т = ¿/т, 2 = 0, т — 1:

1* вычислить сигма-точки из алгоритма 7; 2* найти прогнозные значения сигма-точек

X

к,(Я-1)

i,fc-l|fc-l

4,fc-l|fc-l Ü+l)

2га

3* вектор состояния: = Е X

Ü+l)

i=0

4,fc-l|fc-l>

4* квадратный корень матрицы ковариации ошибки предсказания:

Р,

ü+i)

fc-i|fc-i

1/2

ИЛ(с) X'

(j+i) fc-i|fc-i

e,

где в — ортогональное преобразование, приводящее правую часть к нижнему треугольному виду, а

Lfi£1|fc_i=Lf {^к-Цк-vtk-i+jr

и X'

*,(Я-1) fc-i|fc-i

есть

п х 2п-матрица вида (где i = 1,..., 2п)

см.

продолжение реализаций нового сигма-точечного фильтра Калмана (CD-UKF)

Стандартный СБ-1ЖР (продолжение)

Квадратно-корневой СБ-1ЖР (продолжение)

ю

+

—Lf(j)

32 fc:1|fc"1 СО

Lf'

U)

т

т

+ ü)

к-1\к-1 + - +

рОО 1к-1\к-1

определить прогнозные значения:

где

к-1\к-1) 1 к-1\к-1

GT

(то)

Х.к\к-1 — Рк\к-1 — + к_1\к_у

2. Этап обработки измерений и фильтрации (к = 1 ,...,К).

В момент времени Ьк известны хк\к-\ и Рк\к-1- Для того чтобы вычислить оценки хк\к и Рк\к, повторить этап обработки измерений и фильтрации алгоритма 3 (стандартный ШР).

Р

(то)

r*,(i+l)

v*,U+1) _ - ü'+i) ^k-l\k-l xfc-l|fc-l'

обновить квадратный корень ковариации22 с учетом И7^

Р,

ü+i)

ч 1/2

ITK-iJ =cholupdate (P^-i)

где У обозначает матрицу

ч 1/2т

F =

W,

v^AJ^1-) _ -Л-0 + 1) N ^0,fc-l|fc-l Xfc-l|fc-l

+ I I \l yö (^fc-ilfc-i

U)

L| к

.(то) „1/2

прогноз: Xfcifc.! = x>k_[\k_v P^ =

p

(to)

1/2

к-1\к-1у

1. Этап обработки измерений и фильтрации (к = 1,...,К). Для

вычисления х.к\к и Рщ повторить этап обработки измерений и фильтрации алгоритма 4 (БИ 1ГКР).

Реализации нового обобщенного фильтра Калмана (СО-ЕКЕ)

Стандартный СБ-ЕКР

Алгоритм 9 (СБ-ЕКР). 0. Априорные данные (к = 0).

• Начальные значения: х0|0

хо, Ро|о

Пп

Квадратно-корневой СБ-ЕКР

Алгоритм 10 (БИ СБ-ЕКР). 0. Априорные данные (к = 0).

• Разложение15: П0 = П^Пд72, К = П1/2ПТ/2.

хп Р1/2

II,

1/2

• Начальные значения: х0|0

см. продолжение реализаций нового обобщенного фильтра Калмана (СБ-ЕКР)

223аметим, что так как V — матрица, то малоранговая модификация с]ю1ирс^е квадратного корня выполняется последовательно, т.е. в

цикле, для каждого столбца матрицы V.

Стандартный СБ-ЕКР (продолжение)

1. Этап экстраполяции (к = 1 ,...,К). В момент времени 1 известны и Рк_\\k~\- На временном интервале [¿¿¡-ь^]

предсказать Щк-1 и рк\к-\ = 4 - ^-1):

• Хк-1\к-1 - *к-1\к-Ъ ^к-1\к-1 - к—1\к— 1 •

• Выполнить т-шаговую процедуру обновления оценок,

т = 5/т,] = 0, т — 1: 1* оценка вектора состояния:

= ^ + 3Т

к—1\к—1 ~ '-а 2* ковариация ошибки предсказания:

Ри+1) _ /7 + ^

Г2 Г / / Л \ т + -77-

2

гДе Мк-1\к-г = М (4-1^-1,^-1+^),

• определить прогнозные значения:

_ .(то) р _ р(то)

^к\к-1 — — 11к— 1' Щк-1 — ^к-1\к-Г 1. Этап обработки измерений и фильтрации (к = 1 ,...,К).

В момент времени ¿д. известны 5ск\к-1 и Рк\к-1- Для того чтобы вычислить оценки хцк и Рк\к1 повторить этап обработки измерений и фильтрации алгоритма 1 (стандартный ЕКР).

Квадратно-корневой СБ-ЕКЕ (продолжение)

1. Этап экстраполяции (к = 1 ,...,К). В момент времени 1

1/2

известны ~кк_цк_1 и Рк_1,к_1- На временном интервале

1/2

предсказать ±к\к_г и Рщк_х (6 = tk - tk-1):

>(о)

Р

(0)

1/2

Р,

1/2

Xfc-l|fc-l~ xfc-l|fc-l) ^ J — i

Выполнить m-шаговую процедуру обновления оценок, т = 5/т, j = 0, m — 1:

1* оценка вектора состояния:

х

У+1)

^fc-iifc-i ^id 2* квадратный корень матрицы ковариации:

Р,

ü+i)

fc-i|fc-i

1/2

¿n + TP,

А

G + 2Lfi-i|fc-i

-i|fc-i)

Л 1/2

SKV.

в,

где в — ортогональное преобразование, приводящее правую часть к нижнему треугольному виду. • определить прогнозные значения:

р:

м

1/2

,К).

ъ , _ аМ р!/2 ^fc|fc-l — ^fclfc-l — ^

2. Этап обработки измерений и фильтрации (k = 1,

1/2

В момент времени tk известны ъ.к\к-\ и Для вычисления

1/2

хд.|д. и Pfc|fc повторить этап обработки измерений и фильтрации алгоритма 2 (SR EKF).

£ S

CD £

CD

CD £

1

tu

о &

е-

№ |

¡SJ

ю

сл

Благодарности. Работа выполнена при финансовой поддержке португальского фонда науки и технологии (Fundagao para a Ciencia e a Tecnologia) (гранты № UID/Multi/ 04621/2013, SFRH/BPD/64397/2009, IF/00703/2013).

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

[1] Jazwinsky, A. Stochastic processes and filtering theory. N. Y.: Academic Press, 1970. 376 p.

[2] Kalman, R.E., Bucy, R.S. New results in linear filtering and prediction theory // J. of Basic Engineering. 1961. Vol. 83, No. 1. P. 95-108.

[3] Arasaratnam, I., Haykin, S. Cubature Kalman filters // IEEE Trans. Automat. Control. 2009. Vol. 54, No. 6. P. 1254-1269.

[4] Синицын И.Н. Фильтры Калмана и Пугачева. М.: Университетская книга, Логос, 2006. Sinitsyn, I.N. Kalman and Pugachev filters. Moscow: Univ. Kniga, Logos, 2006. (In Russ.)

[5] Maybeck, P.S. Stochastic models, estimation and control. London: Acad. Press, 1982. 291 p.

[6] Sarkka, S. On unscented Kalman filter for state estimation of continuous-time nonlinear systems // IEEE Trans. Automat. Contr. 2007. Vol. 52, No. 9. P. 1631-1641.

[7] Benzerrouk, H., Nebylov, A. Robust nonlinear filtering applied to integrated navigation system INS/GNSS under non Gaussian measurement noise effect // Proc. of the IEEE "Aerospace Conference", 2012. USA, MT: Big Sky, 2012. P. 1-8.

[8] Benzerrouk, H., Nebylov, A., Salhi, H. Contribution in information signal processing for solving state space nonlinear estimation problems // J. of Signal and Information Processing. 2013. Vol. 4, No. 4. P. 375-384.

[9] Julier, S.J., Uhlmann, J.K., Durrant-Whyte, H. A new approach for filtering nonlinear systems // Proc. of the "American Control Conference". Seattle, WA, 1995. P. 1628-1632.

[10] Julier, S.J., Uhlmann, J.K. A new extension of the Kalman filter to nonlinear systems // Proc. of the Intern. Conf. "Signal Processing, Sensor Fusion, and Target Recognition". Orlando FL, 1997. P. 182-193.

[11] Julier, S., Uhlmann, J., Durrant-Whyte, H. A new method for the nonlinear transformation of means and covariances in filters and estimators // IEEE Trans. Automat. Control. 2000. Vol. 45, No. 3. P. 477-482.

[12] Wan, E.A., Van der Merwe, R. The unscented Kalman filter for nonlinear estimation // Proc. of the Adaptive Systems for Signal Processing, Communications, and Control Symposium. Lake Louise, Alta, 2000. P. 666-672.

[13] Ito, K., Xiong, K. Gaussian filters for nonlinear filtering problems // IEEE Trans. Automat. Control. 2000. Vol. 45, No. 5. P. 910-927.

[14] N0rgaard, M., Poulsen, N.K., Ravn, O. New developments in state estimation for nonlinear systems // Automatica. 2000. Vol. 36. P. 1627-1638.

[15] Arasaratnam, I., Haykin, S., Elliott, R.J. Discrete-time nonlinear filtering algorithms using Gauss —Hermite quadrature // Proc. IEEE. 2007. Vol. 95, No. 5. P. 953-977.

[16] Van der Merwe, R., Wan, E.A. Efficient derivative-free Kalman filters for online learning // Proc. of the European Symposium on Artificial Neural Networks. Bruges, Belgium, 2001. P. 205-210.

[17] Julier, S., Uhlmann, J. Unscented filtering and nonlinear estimation // Proc. IEEE. 2004. Vol. 92, No. 3. P. 401-422.

[18] Van der Merwe, R., Wan, E.A. The square-root unscented Kalman filter for state and parameter-estimation // Proc. of the IEEE Intern. Conf. "Acoustics, Speech, and Signal Processing". Salt Lake City, UT, 2001. Vol. 6. P. 3461-3464.

[19] Liu, G., Worgotter, F., Markelic, I. Square-root sigma-point information filtering // IEEE Trans. Automat. Control. 2012. Vol. 57, No. 11. P. 2945-2951.

[20] N0rgaard, M., Poulsen, N.K., Ravn, O. Advances in derivative-free state estimation for nonlinear systems // Tech. Rep. IMM-REP-1998-15, Tech. Univ. of Denmark, 2000.

[21] Arasaratnam, I., Haykin, S. Square-root quadrature Kalman filtering // IEEE Trans. Signal Processing. 2008. Vol. 56, No. 6. P. 2589-2593.

[22] Tang, X., Wei, J., Chen, K. Square-root adaptive cubature Kalman filter with application to spacecraft attitude estimation // Proc. of the Conf. "Information Fusion (FUSION)". Singapore, 2012. P. 1406-1412.

[23] Chandra, K.P.B., Gu, D.-W., Postlethwaite, I. Square root cubature information filter // IEEE Sensors J. 2013. Vol. 13, No. 2. P. 750-758.

[24] Arasaratnam, I., Haykin, S. Cubature Kalman smoother // Automatica. 2011. Vol. 47. P. 2245-2250.

[25] Wan, E.A., van der Merwe, R. The unscented Kalman filter. Chapter 7: Kalman filtering and neural networks / Ed. S. Haykin. N. Y.: John Wiley & Sons, 2001. P. 221-280.

[26] Arasaratnam, I., Haykin, S., Hurd, T.R. Cubature Kalman filtering for continuous-discrete systems: theory and simulations // IEEE Trans. Signal Processing. 2010. Vol. 58, No. 10. P. 4977-4993.

[27] Конаков А.С., Шаврин В.В., Тисленко В.И., Савин А.А. Сравнительный анализ среднеквадратической погрешности определения координат объекта в бесплатформенной инерциальной навигационной системе при использовании различных алгоритмов нелинейной фильтрации // Докл. Томского гос. ун-та систем управления и радиоэлектроники. 2012. № 1. С. 5-9.

Konakov, A.S., Shavrin, V.V., Tislenko, V.I., Savin, A.A. Comparative analysis of the mean square error determining the coordinates of an object in a strapdown inertial navigation system using a variety of nonlinear filtering algorithms // Proc. of Tomsk State Univ. of Control Systems and Radioelectronics. 2012. No. 1. P. 5-9. (In Russ.)

[28] Сытник А.А., Раевский Н.В., Ключка К.Н., Протасов С.Ю. Сопоставление методов фильтрации в задачах статистической регуляризации при оценивании параметров радиолокационных систем // Вест. Воронежского гос. ун-та. Серия: Системный анализ и информационные технологии. 2013. № 1. С. 10-16.

Sytnik, A.A., Rayevskiy, N.V., Klyuchka, K.N., Protasov, S.Yu. Comparison of methods of the filtration in problems of statistical regularization at estimation of parameters of radar-tracking systems // Proceedings of Voronezh State University. Series: System analysis and information technologies. 2013. No. 1. P. 10-16. (In Russ.)

[29] Kailath, T., Sayed, A. H., Hassibi, B. Linear estimation. N. J.: Prentice Hall, 2000. 854 p.

[30] Grewal, M.S., Andrews, A.P. Kalman filtering: theory and practice. N. J.: Prentice Hall, 2001. 401 p.

[31] Kulikova, M.V., Semoushin, I.V. Score evaluation within the extended square-root information filter // Lecture Notes in Computer Science. 2006. Vol. 3991. P. 473-481.

[32] Kulikova, M.V. Likelihood gradient evaluation using square-root covariance filters // IEEE Trans. Automat. Control. 2009. Vol. 54, No. 3. P. 646-651.

[33] Цыганова Ю.В., Куликова М.В. Об эффективных методах параметрической идентификации линейных дискретных стохастических систем // Автоматика и телемеханика. 2012. № 6. С. 34-51.

Tsyganova, Yu.V., Kulikova, M.V. On efficient parametric identification methods for linear discrete stochastic systems // Autom. Remote Control. 2012. Vol. 73, No. 6. P. 962-975.

[34] Kulikova, M.V., Pacheco, A. Kalman filter sensitivity evaluation with orthogonal and J-orthogonal transformations // IEEE Trans. Automat. Control. 2013. Vol. 58, No. 7. P. 1798-1804.

[35] Tsyganova, J.V., Kulikova, M.V. State sensitivity evaluation within UD based array covariance filter // IEEE Trans. Automat. Control. 2013. Vol. 58, No. 11. P. 2944-2950.

[36] Куликова М.В., Цыганова Ю.В Общий подход к построению алгоритмов параметрической идентификации в классе квадратно-корневых фильтров с ортогональными и J-ортогональными преобразованиями // Автоматика и телемеханика. 2014. № 8. С. 59-81. Kulikova, M.V., Tsyganova, Yu.V. A general approach to constructing parameter identification algorithms in the class of square root filters with orthogonal and J-orthogonal tranformations // Autom. Remote Control. 2014. Vol. 75, No. 8. P. 1402-1419.

[37] Julier, S.J. The scaled unscented transformation // Proc. of the "American Control Conference". Anchorage, 2002. P. 4555-4559.

[38] Frogerais, P., Bellanger, J.-J., Senhadji, L. Various ways to compute the continuous-discrete extended Kalman filter // IEEE Trans. Automat. Control. 2012. Vol. 57, No. 4. P. 10001004.

[39] Kloeden P.E., Platen E. Numerical solution of stochastic differential equations. Berlin: Springer, 1999. 856 p.

Поступила в 'редакцию 15 декабря 2015 г., с доработки — 29 января 2016 г.

Numerical methods for nonlinear filtering of signals and measurements

Kulikova, Mariya V.*, Kulikov, Gennadiy Yu.

CEMAT, Instituto Superior Tecnico, Universidade de Lisboa, Av. Rovisco Pais 1, 1049-001 Lisboa

* Corresponding author: Kulikova, Mariya V., e-mail: maria.kulikova@ist.utl.pt

This paper studies numerical methods of contemporary nonlinear Kalman filtering for estimation of unknown vector of state in stochastic continuous-time systems presented by Ito-type stochastic differential equations with discrete measurements. The elaborated methods are analysed and compared in the case of severe conditions of tackling a seventh-dimensional radar tracking problem, where an aircraft executes a coordinated horizontal turn. The latter problem is considered to be a challenging example for testing nonlinear filtering algorithms. This paper explores such effective state estimation methods as the cubature and unscented Kalman filters, including their square-root versions. Implementation particulars and performances of the mentioned techniques are studied for various values of aircraft's turn rate and sampling time. New variants of the extended and unscented Kalman filters are also presented for treating continuous-discrete stochastic systems. It is shown that the new methods outperform the traditional extended Kalman filter in the considered air traffic control scenario.

Keywords: Ito-type stochastic differential equations with discrete measurements, optimal estimate of the state vector of stochastic system, extended Kalman filter, unscented Kalman filter, cubature Kalman filter.

Acknowledgements. The authors thank the support from Portuguese National Funds through the Fundacao para a Ciencia e a Tecnologia (FCT) within projects UID/Multi/ 04621/2013, SFRH/BPD/64397/2009 and the Investigador FCT 2013 programme.

Received 15 December 2015 Received in revised form 29 January 2016

© ICT SB RAS, 2016