CC BY
0
Научная статья
УДК 656.052.1
DOI: 10.14529/йо240404
БЛОЧНЫЙ ФИЛЬТР КАЛМАНА С ЛИНЕЙНОЙ ВЫЧИСЛИТЕЛЬНОЙ СЛОЖНОСТЬЮ ДЛЯ КОМПЛЕКСИРОВАННЫХ С ТЕХНИЧЕСКИМ ЗРЕНИЕМ ИНЕРЦИАЛЬНЫХ НАВИГАЦИОННЫХ СИСТЕМ
Н.И. Циоплиакис, [email protected], https://orcid.org/0009-0008-0530-0500 Южно-Уральский государственный университет, Челябинск, Россия
Аннотация. В условиях роста применения технического зрения в навигационных системах подвижных объектов возникает необходимость в исследовании численно-эффективных алгоритмов оптимальной фильтрации, адаптированных для совместной обработки измерений инерциальных датчиков и данных большого числа визуальных источников информации. В статье предложена новая модификация фильтра Калмана (блочный фильтр Калмана), имеющая линейную вычислительную сложность от числа источников измерительной информации; получена численно-устойчивая версия фильтра, использующая LDL-факторизацию матриц ковариаций. Цель исследования: получить алгоритм LDL-факторизованного блочного фильтра Калмана, имеющего линейную вычислительную сложность относительно числа входящих в измерительную систему источников информации; показать возможность применения такого алгоритма в комплексированной инерциальной навигационной системе с техническим зрением. Материалы и методы. Доказывается алгебраическая эквивалентность блочного фильтра Калмана классическому в специальном случае; предлагается способ приближения оценок блочного фильтра к оценкам классического с требуемой точностью за счет расширения вектора состояния. Вычислительная сложность блочного фильтра Калмана сравнивается с вычислительной сложностью классического фильтра Калмана в рамках численного эксперимента. Проводится численное моделирование работы блочного фильтра в составе навигационной системы для сравнения с классическим фильтром. Результаты. Получены уравнения блочного LDL-факторизованного фильтра Калмана; проверена его линейная вычислительная сложность от числа источников информации. Предложен и проверен в рамках моделирования работы навигационной системы способ приближения оценок блочного фильтра Калмана к оценкам классического фильтра за счет расширения вектора состояния. Заключение. Основные теоретические свойства блочного фильтра Калмана были подтверждены в рамках численных экспериментов. В ходе дальнейших исследований будут рассмотрены альтернативные способы формирования расширенного вектора состояния фильтра; будет проведено тестирование блочного фильтра Калмана в рамках более сложных сценариев.
Ключевые слова: фильтр Калмана, численная эффективность, техническое зрение, комплекси-рованная навигационная система
Благодарности. Работа выполнена в рамках государственного задания Минобрнауки России FENU-2024-0004 от 17.01.2024.
Для цитирования: Циоплиакис Н.И. Блочный фильтр Калмана с линейной вычислительной сложностью для комплексированных с техническим зрением инерциальных навигационных систем // Вестник ЮУрГУ. Серия «Компьютерные технологии, управление, радиоэлектроника». 2024. Т. 24, № 4. С. 43-56. DOI: 10Л4529/йсг240404
© Циоплиакис Н.И., 2024
Original article
DOI: 10.14529/ctcr240404
BLOCK KALMAN FILTER WITH LINEAR COMPUTATIONAL COMPLEXITY FOR INTEGRATED VISUAL-INERTIAL NAVIGATION SYSTEMS
N.I. Tsiopliakis, [email protected], https://orcid.org/0009-0008-0530-0500 South Ural State University, Chelyabinsk, Russia
Abstract. As the use of computer vision in vehicle navigation systems increases, there is a growing need for computationally efficient optimal filtering algorithms that can jointly process measurements from inertial sensors and a large number of visual information sources. Contribution. This paper proposes a new modification of the Kalman filter (the block Kalman filter) with linear computational complexity in the number of measurement information sources. A numerically stable version of the algorithm is derived using LDL-factorization of covariance matrices. Purpose of the study. The aim is to develop an LDL-factorized block Kalman filter algorithm with linear computational complexity with respect to the number of information sources in the measurement system and demonstrate its applicability in a integrated visual-inertial navigation system. Materials and Methods. This research demonstrates the algebraic equivalence of the block Kalman filter to the standard one in a specific case. A method is proposed for approximating the estimates of the block filter to those of the standard one with desired accuracy by expanding the state vector. The computational complexity of the block Kalman filter is compared to the computational complexity of standard one within a numerical experiment. Numerical modeling of the block filter operation within a navigation system is conducted for comparison with the standard filter. Results. The equations of the block LDL-factorized Kalman filter are obtained. Its linear computational complexity with respect to the number of information sources is verified. A method for approximating the estimates of the block Kalman filter to those of the standard filter by expanding the state vector is proposed and verified within the framework of navigation system simulation. Conclusion. The main theoretical properties of the block Kalman filter were confirmed in numerical experiments. Further research will explore alternative methods of forming the extended state vector of the filter; the Kalman block filter will be tested within more complex scenarios.
Keywords: Kalman filter, numerical efficiency, computer vision, integrated navigation system
Acknowledgments. The work was carried out within the framework of the state assignment of the Ministry of Science and Higher Education of the Russian Federation No. FENU-2024-0004 dated 01/17/2024.
For citation: Tsiopliakis N.I. Block Kalman filter with linear computational complexity for integrated visual-inertial navigation systems. Bulletin of the South Ural State University. Ser. Computer Technologies, Automatic Control, Radio Electronics. 2024;24(4):43-56. (In Russ.) DOI: 10.14529/ctcr240404
Введение
Внедрение мобильных роботизированных систем в такие отрасли современной экономики, как транспорт, логистика и диагностика сопровождается повышением требований к надежности их навигационного обеспечения, что обуславливает использование визуальных источников корректирующей информации (техническое зрение) наряду с классическими (приемниками спутниковых сигналов, дальномерами).
Специфика использования технического зрения в навигации заключается в необходимости совместной обработки данных большого и переменного числа отслеживаемых визуальных признаков, которые выступают в качестве источников навигационной информации. В современных комплексированных с техническим зрением инерциальных навигационных системах для получения оптимальных оценок положения и ориентации подвижного объекта применяются рекурсивные фильтры, такие как фильтр Калмана [1-3], фильтр частиц [4, 5] и их модификации, а также методы нелинейной оптимизации (по окну, включающему некоторое число измерений) [6]. При этом, несмотря на развитие альтернативных подходов, калмановская фильтрация продолжает широко использоваться в данной области [7].
Для работы навигационной системы в реальном времени важно обеспечить быстрое функционирование комплексирующего фильтра, тогда как для улучшения характеристик точности и
надежности оценок требуется обрабатывать большее число визуальных признаков. Степенная вычислительная сложность от числа источников информации, характерная для многих оптимальных фильтров (в частности, кубическая сложность классического фильтра Калмана [8]), усугубляет противоречие между этими требованиями, заставляя искать субоптимальные алгоритмы фильтрации с меньшими затратами на вычисления.
Для задачи комплексирования инерциальных данных и данных технического зрения известен класс фильтров с линейной вычислительной сложностью, называемых Multi-State Constraint Kalman filters (MSCKF, фильтры Калмана с ограничениями на множественные состояния системы) [9]. В рамках данных фильтров оценки расположения неподвижных визуальных признаков рассчитываются однократно на этапе их инициализации вне фильтра Калмана, векторы ошибок расположения признаков не включаются в вектор состояния фильтра и не оцениваются. Ряд последовательных наблюдений одного и того же визуального признака объединяется в общее уравнение измерений, после чего данное уравнение проецируется на левое нуль-пространство матрицы измерений вектора ошибок расположения визуального признака; таким образом, он исключается из общего уравнения измерения. В результате в уравнение измерений входят лишь векторы ошибок оценок положения и ориентации навигационной системы в последовательные моменты времени [7, 9].
Среди недостатков MSCKF можно назвать то, что векторы состояния визуальных признаков не оцениваются, что не позволяет его применять в системах, где у данных векторов имеется динамика (например, если расположение визуальных признаков задается в подвижной системе координат). Также это вызывает повышенную погрешность от линеаризации, так как матрицы измерений рассчитываются путем линеаризации в точке, соответствующей начальной оценке расположения визуального признака. Кроме того, несмотря на линейную сложность от числа визуальных признаков, объем требуемых вычислений и затрат памяти довольно велик в связи с необходимостью построения общих матриц измерения фильтра, объединяющих уравнения измерения в последовательные моменты времени.
В данной статье предложен фильтр, также обладающий линейной вычислительной сложностью относительно числа источников информации, но позволяющий оценивать полный вектор состояния, включая векторы состояния источников информации. В отличие от MSCKF данный фильтр на каждой своей итерации использует только текущие уравнения измерения (как классический фильтр Калмана), снижение вычислительной сложности достигается за счет упрощения структуры фильтра при принятии некоторых допущений, что созвучно с подходами, применяемыми в распределенных фильтрах Калмана (Distributed Kalman Filters).
Различные алгоритмы распределенных фильтров известны и для фильтра Калмана, и для фильтра частиц [10]. Они рассматривались как способ понижения вычислительной сложности до квадратичной, способ параллелизации вычислений [11], а также как способ организации обработки данных мультисенсорных сетей [12-15]. Данные алгоритмы оперируют отдельными блоками, выделенными из матриц классического фильтра Калмана, позволяя «развязать» уравнения, связанные с каждым из источников информации. Однако распределенные фильтры из работ [12-14] эффективны только в случае, когда измерители не имеют своего вектора состояния (связанного только с данным измерителем); таким образом, они не подходят для использования в комплекси-рованных с техническим зрением инерциальных навигационных системах.
1. Структура уравнений измерения и принятые статистические допущения
В данной статье рассматривается система со следующей структурой уравнения измерений:
Y = v - HX,
■( 0, R ) =
(1)
" X1 " H12 H 2 0 • •• 0 " v 2 " R 2 0 • ■ ■ 0 "
X = X 2 , H = H13 0 H3 • •• 0 , v = V3 , R = 0 R3 • ■ ■ 0
_ X N _ _H1N 0 0 • " H N _ _ V N _ 0 0 • " R N _
где Y - вектор невязки измерений; X - вектор состояния; X! - общий вектор состояния системы; Хг- - вектор состояния /-го измерителя, / = 2...N ; Н - матрица измерений; Н1г- - матрица
v
измерений общего вектора состояния системы i-м измерителем; Н, - матрица измерений вектора состояния /-го измерителя; V - вектор шумов измерений; V, - вектор шумов измерений /-го измерителя; Хх ~ (0, Рп ) - общий вектор состояния системы; X, ~ (0, Р//) - вектор состояния /-го измерителя.
Уравнения блочного фильтра Калмана получим, приняв в качестве допущения, что векторы состояния измерителей статистически связаны друг с другом лишь посредством общего вектора состояния системы, т. е. выполнены условия (2).
Статистически связанными посредством X! будем называть такие векторы X,, Xу, для
которых выполняется:
X/ = Л/А + у,, / = 2... N,
Xу = Л у^ + , у = 2.М, у Ф/, (2)
ССу ( У /, ^) = ССу (у}, ^) = ССу (У /, у}) = 0.
В специальном случае, когда условия (2) выполнены, блочный фильтр Калмана будет алгебраически эквивалентен классическому. Этот случай интересен тем, что матрицы взаимной корреляции Ру между векторами состояния любых двух различных измерителей X;, Xj выражаются
через матрицы Р/1, Р1 у их взаимной корреляции с X! и его ковариационную матрицу Рп :
Ру = ССУ^., Xу) = Р/1РП-1Р1 у, где Р/1 = ccy(X/, Рп = ccy(Xl, Xl), Р1} = ccy(Xl, X}).
Следовательно, в описываемом случае матрицы взаимной корреляции между векторами состояния измерителей Ру не несут дополнительной информации, что указывает на возможность
их исключения из алгоритма фильтра Калмана, существенно упростив его структуру.
2. Редукция порядка фильтра Калмана, получение блочных уравнений этапа коррекции
Запишем уравнение невязки измерений /-го измерителя:
Yi = V/ - ИцX - Н/ X/,
V, ~ (0,R/), ссу(V,,Vy ) = 0, / = 2.N, у = 2...N, у Ф /. (3)
В силу условий (2) вектор состояния /-го измерителя X, можно разложить на составляющую, пропорциональную вектору состояния Xl, и остаток у,, некоррелированный с векторами состояния остальных измерителей:
X/ = РяРйХ + у,, X/ ~ (0,Р//), у, ~ (0,Р/1), / = 2...N, (4)
где Р/ = Р// - Р/1Р111Р1/.
В (3) выделим из X, коррелированную с Xl составляющую, сгруппируем остаток у, с V,: V/ = (V/ - И/У/) - (Н1/ + И/ад-1)Xl, / = 2...N . (5)
Введем обозначения:
V/ - Н/У/), V/ ~ (0,/), / = 2. N, (6)
где 1 / = 1/ +Н/Р/^Н[ .
По построению V/ не коррелирован с Vу- при /' ф у и может рассматриваться как приведенный шум измерений /-го измерителя, тогда 11, - матрица ковариаций приведенного шума /-го измерителя.
Тогда невязку измерений /-го измерителя можно переписать в виде
У, = V/ - Н1// = 2. N, (7)
где !Н1/- = Н1/ + Н,Р/1Р1-1 - приведенная матрица измерений.
Таким образом, из всех уравнений измерений были исключены векторы состояния измерителей; вектор состояния новой системы равен общему вектору состояния X! исходной системы.
Тогда для расчета апостериорной матрицы ковариаций оптимальной оценки общего вектора состояния X! можно воспользоваться ее выражением в информационной форме [16]:
Гц
( N У1
Р11 + Х Н1К- Н1
(8)
1=2 )
Зная апостериорную матрицу ковариаций оптимальной оценки общего вектора состояния Х1, матрицы оптимальных коэффициентов Калмана для него можно выразить как
Кц. = РцНЦЯ-1, 1 = 2...N. (9)
Таким образом, порядок фильтра Калмана удалось редуцировать, исключив векторы состояния измерителей, и на основе редуцированной модели были получены оптимальные коэффициенты Калмана для общего вектора состояния Х1. Тогда оптимальная апостериорная оценка Х1 выражается как
_ N
X = Х1 + X ВД, (10)
1=2
где х1 - оптимальная апостериорная оценка общего вектора состояния.
Далее необходимо рассчитать коэффициенты Калмана для всех векторов X., используя знание х1 .
Оптимальную апостериорную оценку x. можно записать в виде
X. = РцРп1^ + у. + К1 (у. -НН11X1 -Н.у.), 1 = 2...N, (11)
где К. - матрица оптимальных коэффициентов усиления по невязке .-го уравнения измерения.
Сгруппируем в (11) члены, содержащие xl, и введем обозначения:
x. = м.^ + у., . = 2...N , (12)
где М. = РдР-1 -К.Нц, у, =(11К.Н.)у. + К.V..
Таким образом, оптимальная апостериорная оценка x. состоит из двух частей: части, линейно зависящей от xl, и остатка у.. Поскольку оценка xl уже оптимальна, 1С. можно определить из условия минимизации следа матрицы ковариации остатка у.:
!К.. = Р!Н[ (Н.Р*.Н[ + Я.)-1, . = 2...N . (13)
Условия (2) требуют, чтобы апостериорные оценки векторов состояния измерителей x. были статистически связаны посредством xl, иначе условия применения блочного алгоритма фильтра Калмана не будут выполнены на следующей итерации фильтра:
соу^.Д ) = 0, . = 2. N, (14)
соу(ъ,у) = 0, . = 2.N, ] *.. (15)
Вычислим соу (у., У] ) и покажем, что это условие (15) выполнено:
соу(ъ,у]) = (Ъ ■ ч]) =
(I-КН)У; + КК.V..]{(1-К]Н])у] + К]V]= 0, . = 2.N,]*.. (16)
Вычислим соу ( У,-, X.
(ъ, Xl):
cov(Y,Xi) = (Y • XT) =
"(I -К,H,)Yi + К,v,'
у n
V ¿=2
N
I -Z KuHi, Xi + £ K, (v, - H,Y, )
1С, R К-(I - К, H,) Piy HT Ki; =[k , (h, Piy HT + R,)-Piy HT ] KT, , = 2... N.
,=2 +;
(17)
Нетрудно видеть, что подстановка в (17) выражения (13) для оптимального коэффициента К , обращает (17) в 0, то есть (14) выполнено.
Таким образом, условия (2) не нарушаются в результате введения оптимальных поправок, и алгоритм блочного фильтра может быть применен и на следующих итерациях. Осталось рассчитать матрицы ковариаций для апостериорных оценок X,:
pi = m, p11;
Py =(i - k, h, ) pj , p,, = m, PiiMT + pj .
(i8)
В соответствии с (11) получаем, что рассчитать коэффициенты Калмана вектора состояния /-го измерителя X, можно по следующим формулам:
у Ф /
К =
M,Ki j,
М,Кц + К,,
(i9)
j = ;
В соответствии с (11) и (19) оптимальную апостериорную оценку X, вектора состояния
-го измерителя можно получить как
_ N _
X = X, + £ М/К1у Уу + К, У = M/AX1 + К/У/, (20)
у=2
_ N
где AXl = £ Кц у Уу - оптимальная поправка к оценке общего вектора состояния системы Xl.
у=2
Таким образом, с учетом статистических взаимосвязей между оценкой общего вектора состояния системы XI и оценками векторов состояния измерителей X,, / = 2.N, заданных условиями (2), получены упрощенные выражения для расчета оптимальных оценок. Сложность алгоритма линейна относительно числа измерителей. Графически отобразить отличие предложенных уравнений от используемых в классическом алгоритме фильтра Калмана позволяют схемы на рис. 1 и 2.
Поправки к оценкам векторов состояния блоков
Блок i (общий вектор состояния системы)
Блок 2 (измеритель)
Блок 3 (измеритель)
Блок N (измеритель)
r- Невязки уравнений
! Ki2 измерении
Ki
Блок 2 (измеритель)
Блок 3 (измеритель)
Блок N (измеритель)
Рис. 1. Схема расчета оптимальных корректирующих поправок блочного фильтра Калмана Fig. 1. The scheme for calculating optimal corrections of the block Kalman filter
T
Поправки к оценкам векторов состояния блоков
Блок 1 (общий вектор состояния системы)
Блок 2 (измеритель)
Невязки уравнений измерений
Блок 2 (измеритель)
Блок 3 (измеритель)
Блок N (измеритель)
Рис. 2. Схема расчета оптимальных корректирующих поправок классического фильтра Калмана Fig. 2. The scheme for calculating optimal corrections of the standard Kalman filter
3. Этап прогноза
Блочные уравнения этапа прогноза получены на основе уравнения прогноза для матрицы ко-вариаций (21).
P = FPFT + Q, F =
f F11 F12 F ^ F1N f Q11 Q12 Q1N ^
F21 F22 0 0 , Q = Q21 Q22 0 0
0 0 0 0
ч FN1 0 0 fnn j v Q N1 0 0 Q NN J
(21)
где Р - новое значение матрицы ковариаций; F - переходная матрица динамики ФК; Q - матрица ковариаций шумов динамики ФК.
Уравнения прогноза для матриц ковариаций фильтра можно записать как
P11 = F11P11F/1 + Q:
H1-
P = (F.A, + FP ) • FT + (FllPn + F,Pfl) • Ffl + Q,,
_ N
P1 = (F 1P 1 + F, P11) FT1 + (F, P1P111 + F,1) • X [P1 j ^ ] + Q1,
где Fn = F,, +
N
X f p.
i=2
N t \
• Pn1; = Q11 + XF1(p, - адМ )Ft .
i=2
Динамика общего вектора состояния системы X! в общем случае приводит к нарушению условий (2), даже если в начальный момент они были выполнены, так как векторы состояния измерителей Хг-, X^ становятся статистически связаны не только посредством текущего значения
общего вектора состояния системы, но и посредством его значений в прошлом:
X) = A^X) + A)-1X)-1 +... + A01X0 + Y.., i = 2... N, X) = AjX + A)-*)-1 + ...+ A^1X? + Yi, j Ф i, cov (Y i, X1) = cov (Y j, X1) = cov ( y ., Y j ) = 0, s = 0...k.
(22)
В системах без шума динамики общего вектора состояния системы (Q11 = 0) с невырожденными переходными матрицами динамики Fll и при Flг■ = 0, ■ = 2...N текущее значение общего вектора состояния системы определяет все его состояния в прошлом:
X? -1 = А^Х?, X? "2 = А^Х?, ..., х0 = А01хк. (23)
Подставив (23) в (22), получаем, что в этом случае условия (2) выполнены:
Xk = A,X + y,, i = 2...N,
Xkj = A ;1Xf + у
ДХ1 ■ 1 j , j *i, j = 2- N,
cov ( Yi, Xf ) = cov ( Y ;, Xf ) = cov ( Yi, Y ; ) = 0,
(24)
где Aii = Aki + A^A*-1 + ...+ A°A0i:
A = Ak + A k-1Ak-1 + A ji~ A ji + A ji A11 + •
+ A0 A0 ■ A jiAii.
При малых Qll условия (2) выполнены приближенно. Чтобы повысить точность выполнения условий (2), можно расширить общий вектор состояния, выбрав переходные матрицы дополнительных переменных состояния таким образом, чтобы «запоминать» больше информации о прошлых состояниях системы.
Например, для навигационной системы с техническим зрением можно прибегнуть к расширению вектора состояния набором из нескольких НЧ-фильтрованных оценок ошибок координат и углов ориентации, выбрав различные постоянные времени:
XX1 =
T T T T
Xvr9 vr9 vr9
1 X^ X^ ... Xrp
1 T T T
где Х1 - расширенный общий вектор состояния системы; Х1Ф- вектор состояния НЧ-фильт-рованной с постоянной времени Т1 оценки шибок координат и углов ориентации.
Оценки X7 подчиняются следующим уравнениям динамики:
хгф = k+1 "
1
T
1 /
• ХТф k + — • Хкф, i = 1...И,
TikT k
(25)
где Хгф - вектор состояния ошибок оценок координат и углов ориентации НС; Лt - шаг по времени между итерациями фильтра Калмана.
Предварительные численные эксперименты показывают, что для получения оценок навигационных параметров с СКО, эквивалентной СКО оценок классического фильтра Калмана, число п можно выбрать небольшим: п = 1.. .3.
4. LDL-факторизация уравнений блочного фильтра Калмана
В настоящее время разработаны алгоритмы калмановской фильтрации, алгебраически эквивалентные классическому фильтру Калмана, но обладающие значительно большей вычислительной устойчивостью: квадратноокорневой фильтр Поттера, fTD-фильтр Бирмана и др. [8, 17, 18]. Данные алгоритмы используют различные приемы факторизации ковариационных матриц для устранения опасности потери ими положительной определенности и симметричности вследствие ошибок округления.
Для реализации численно-устойчивого алгоритма блочного фильтра Калмана была выбрана LDL-факторизация ковариационных матриц [19].
Введем обозначения: Ln, Dn - нижнетреугольный и диагональный факторы матрицы кова-риаций Pj соответственно; L^, D^ - нижнетреугольный и диагональный факторы матрицы ко-вариаций Pii соответственно; Li1 - фактор, определяющий взаимную корреляцию X1 и Xi : P11 =LnDnLTn, Pi = LnDnL;, i = 2... N,
L L,
11
0
"1х",-L
D
11
"i х"1
0
"1x"i D
l11 0"ix",- T P11 pi,
_ L,1 l,, . _ Pi Pi _
T
4.1. Уравнения LDL-факторизованного блочного фильтра Калмана на этапе коррекции
wr, — [lr,,HL,,], dr, =[dr,,D],
lr,dr, = mwgSL (wr,,dr, ), i = 2.N,
W-1 =
т -T úTt -T úTT -T úTT -T L11 , H 2 lír2, H3 Lr3, •••,H N lt
n r
N
D0 i pii1
D-í, ^ ^
, D
-1
Upi-i, dp-i = (Wpii, dpi-),
Lii — (upi-i)" , Dn — (dp-i Гi,
Ki, = ЦА^Н^-ГDr.lr , i — 2„.N,
Wsj = [H,L",LR,], d0y =[D-,DR,], LSY,Dsy = mwgsL (wsy ,dSy ), i = 2...N,
K, = L,.,.D,,LT,,HTL-TD-1L-1, i — 2...N,
, ii ,, ,, , sY sY sY
WPY — [M, H,) Lii, K i Lr,
L,,, D,, = mwgsL ( W^ , DpY ),
D0 = i — 2... N
[Dii, dr, ] ,
lгl = мг ьц,
где И1г = И1г + ИгЬг1Ь11; Мг = ЬдЬ- -1СгIIг1; mwgsL (W, D0 ) , mwgsU (W,D0 ) - процедуры модифицированной взвешенной ортогонализации Грама - Шмидта с нижнетреугольной и верхнетреугольной матрицами перехода соответственно.
4.2. Уравнения LDL-факторизованного блочного фильтра Калмана на этапе прогноза
^п^Ь0п ], ^ =[Dn,В0п],
Lii,Dii, V^ = mwgsL (WPii,Dpii ) ,
Wp =
x 11
F,1L11 + FiiLi1, L?1, FiiLii, lq„
D
[d11, dqii, d«, dqii] =
L,,, D„
p,v — I di i, dqi = 2. N
mwgs2L (wph , dph ,vpn, Dii ), -где mwgs2L (W, D0, V, D1 ) - процедура модифицированной взвешенной ортогонализации Грама -Шмидта с нижнетреугольной матрицей перехода, достраивающая ортогональную систему векторов-строк v до ортогонального базиса системы векторов-строк VT, WT
Уравнения приведены для случая е-,- — 0, i — 2„. N . Полученный LDL-факторизованный блочный фильтр Калмана имеет линейную вычислительную сложность относительно числа измерителей и линейные затраты памяти.
5. Численные испытания блочного фильтра Калмана
В среде GNU Octave сформирована процедура, реализующая алгоритм блочного LDL-факторизованного фильтра Калмана; подготовлен скрипт, инициализирующий матрицами со случайными коэффициентами структуру фильтра, состоящего из заданного числа блоков с заданными размерностями векторов состояния и измерения.
Проведено сравнение затрат времени на 1 шаг коррекции для блочного LDL-фак-торизованного фильтра Калмана и классического LDL-факторизованного фильтра Калмана на одинаковых исходных данных (рис. 3). Размерность общего вектора состояния принята рав-
N
ной 27, размерность вектора состояния измерителей - 6, размерность вектора измерения измерителей - 8.
0.7 0.6 0.5 0.4 0.3 0.2 0.1 0
0 10 20 30 40 50 60 70 80 90 100 Количество блоков фильтра
Рис. 3. Сравнение вычислительной сложности алгоритмов ФК Fig. 3. Computational complexity comparison of the Kalman Filter algorithms
Данные рис. 3 подтверждают линейную вычислительную сложность алгоритма блочного LDL-факторизованного фильтра Калмана и его превосходство по скорости работы над стандартной реализацией LDL-факторизованного фильтра Калмана.
Затраты времени на 1 шаг коррекции, с
1 ! i ! l i i i i
Классический LDL ФК
i i i i i
6. Пример применения блочного фильтра Калмана
Рассмотрим пример применения блочного фильтра Калмана в навигационной системе (НС) с коррекцией по визуальным опорным маркерам (в рамках численного эксперимента). Данная навигационная система содержит БИНС с МЭМС-датчиками, видеокамеру. При попадании опорного маркера в область зрения видеокамеры происходит оценивание его расположения методами проективной геометрии (Р3Р) [20], после чего координаты угловых точек маркера на кадрах видеопотока используются в качестве измерений в фильтре Калмана для коррекции ошибок БИНС и уточнения оценок расположения маркера.
Вектор состояния навигационной системы включает в себя ошибки БИНС (ошибки оценок ориентации, положения и скорости, смещения нуля МЭМС-датчиков), ошибки оценок расположения опорных маркеров:
X
BINS 15x1
§ФГ 3x1
SrT
3x1
SvT
3x1
bgyr 3x1
b
acc T 3x1
X
marker i 6x1
marker i ^3x1
Sr
marker i 3x1
X
INS
X
BINS J 15x1
marker1J 6x1
marker m J 6x1
^(15+6 т )х1 = Х15х1 Х6х1 Х6х1 , (26)
где 8ф - вектор ошибок оценки ориентации; Ъг - вектор ошибок оценки положения; - вектор ошибок оценки линейной скорости; - вектор смещений нуля датчика угловой скорости; Ьасс - вектор смещений нуля акселерометра; 8фтагкег 1 - вектор ошибок оценки ориентации
■-го маркера; Ъгтагкег ■ - вектор ошибок оценки положения /-го маркера.
В рамках численного эксперимента рассмотрим оценки классического фильтра Калмана с вектором состояния (26), блочного фильтра Калмана с вектором состояния (26) и блочного фильтра Калмана с расширенным вектором состояния (27):
1 т
X
INS+
■( 27+6 m )x1
X
BINS+ 27x1
т т
marker1
X
6x1
X
Л.,
т
marker m
X
BINS+
X
BINS T л^гф T
15x1
X
T 6x1
T
X ГФ
AT2 6x1
(15+6-2 )x1
где s = 1...2, At = 0,05c, T1 = 5c, T2 = 10c
6x1
X r Ф
^T, к+1
(27)
At
- x:
r ф Ts к
+ -
At
5ф Sr
T
T
Покажем, что расширение вектора состояния приближает оценки блочного фильтра Калмана к оценкам классического алгоритма Калмана.
При обзоре одного маркера оценки трех фильтров совпадут, так как в этом случае фильтры алгебраически эквивалентны; поэтому произведем моделирование работы НС при обзоре двух маркеров. На рис. 4 описаны траектория движения объекта с НС, расположение опорных маркеров, начальные и конечные оценки их расположения.
0.5 х 0
Рис. 4. Траектория движения объекта и расположение опорных маркеров, м: а - траектория; b - расположение маркеров и их оценка в начальный момент времени; c - в конечный момент времени Fig. 4. Trajectory of the object and location of the fiducial markers, meters: a - trajectory; b - location of markers and their estimation at the initial time; c - at the final time
На рис. 5 представлены графики оценок навигационных параметров, полученных классическим ФК, блочным ФК без расширения вектора состояния; блочным ФК с расширенным вектором состояния. Как видно из рис. 5, для навигационных параметров с малым временем корреляции ошибки (координаты по оси X, Y, углы тангажа и крена) оценки трех фильтров близки, тогда как для навигационных параметров с большим временем корреляции ошибки (высота, угол рыскания) блочный фильтр Калмана без расширения вектора состояния дает отличающиеся оценки. Это связано с тем, что данный фильтр не учитывает коррелированность векторов состояния ошибок оценок расположения маркеров через «прошлое» общего вектора состояния системы. Из рис. 5 также видно, что расширение вектора состояния блочного фильтра существенно приблизило его оценки к оценкам классического ФК, так как в этом случае условия (22) приближаются к условиям (2).
Высота, м Угол рыскания, град.
Ground truth -KF -BKF----BKF+
Рис. 5. Сравнение навигационных оценок, полученных тремя фильтрами: классическим ФК (KF), блочным ФК (BKF), блочным ФК с расширенным вектором состояния (BKF+) Fig. 5. Comparing navigation estimates from three Kalman Filters: standard KF (KF), block KF (BKF), block KF with extended state vector (BKF+)
Заключение
Были получены уравнения блочного фильтра Калмана; доказана эквивалентность блочного фильтра Калмана классическому при выполнении условий (2). Для улучшения численной устойчивости алгоритма была проведена LDL-факторизация уравнений блочного фильтра Калмана; в рамках численного эксперимента показана его линейная вычислительная сложность от числа источников информации. Предложен способ приближения оценок блочного фильтра Калмана к оценкам классического фильтра за счет расширения вектора состояния; способ проверен в рамках численного эксперимента на примере навигационной системы с коррекцией по опорным маркерам.
В ходе дальнейших исследований будут рассмотрены альтернативные способы формирования расширенного вектора состояния фильтра; будет проведено тестирование блочного фильтра Калмана в рамках более сложных сценариев.
Список литературы
1. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback / M. Bloesch, M. Burri, S. Omari et al. // The International Journal of Robotics Research. 2017. Vol. 36 (10). P. 1053-1072. DOI: 10.1177/0278364917728574
2. Li M., Mourikis A.I. Improving the accuracy of EKF-based visual-inertial odometry // 2012 IEEE International Conference on Robotics and Automation. Saint Paul, MN, USA, 2012. P. 828-835. DOI: 10.1109/ICRA.2012.6225229
3. Fang Q., Huang S.X. UKF for Integrated Vision and Inertial Sensors Based on Three-View Geometry // IEEE Sensors Journal. 2013. Vol. 13 (7). P. 2711-2719. DOI: 10.1109/jsen.2013.2259228
4. Bleser G., Strickery D. Using the marginalised particle filter for real-time visual-inertial sensor fusion // 2008 7th IEEE/ACM International Symposium on Mixed and Augmented Reality. Cambridge, UK, 2008. P. 3-12. DOI: 10.1109/ISMAR.2008.4637316
5. Particle filter approach to vision-based navigation with aerial image segmentation / K. Hong, S. Kim, J. Park, H. Bang // Journal of Aerospace Information Systems. 2021. Vol. 18 (12). P. 1-9. DOI: 10.2514/1.I010957
6. A review of visual inertial odometry from filtering and optimisation perspectives / J. Gui, D. Gu, S. Wang, H. Hu // Advanced Robotics. 2015. Vol. 29 (20). P. 1289-1301. DOI: 10.1080/01691864.2015.1057616
7. Huang G. Visual-Inertial Navigation: A Concise Review // 2019 International Conference on Robotics and Automation (ICRA). Montreal, QC, Canada, 2019. P. 9572-9582. DOI: 10.1109/ICRA.2019.8793604
8. Цыганова Ю.В., Куликова М.В. О современных ортогонализованных алгоритмах оптимальной дискретной фильтрации // Вестник ЮУрГУ. Серия «Математическое моделирование и программирование». 2018. Т. 11, № 4. С. 5-30. DOI: 10.14529/mmp180401
9. Mourikis A.I., Roumeliotis S.I. A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation // Proceedings 2007 IEEE International Conference on Robotics and Automation. Rome, Italy, 2007. P. 3565-3572. DOI: 10.1109/ROBOT.2007.364024
10. INS/vSLAM system using distributed particle filter / D.H. Won, S. Chun, S. Sung et al. // International Journal of Control Automation and Systems. 2010. Vol. 8. P. 1232-1240. DOI: 10.1007/s12555-010-0608-7
11. Paik B.S., Oh J.H. Gain fusion algorithm for decentralised parallel Kalman filters // IEE Proceedings - Control Theory and Applications. 2000. Vol. 147 (1). P. 97-103. DOI: 10.1049/ip-cta:20000141
12. Hashemipour H.R., Roy S., Laub A.J. Decentralized structures for parallel Kalman filtering // IEEE Transactions on Automatic Control. 1988. Vol. 33, no. 1. P. 88-94. DOI: 10.1109/9.364
13. Rao B.S.Y, Durrant-Whyte H.F., Sheen J.A. A Fully Decentralized Multi-Sensor System for Tracking and Surveillance // The International Journal of Robotics Research. 1993. Vol. 12 (1). P. 20-44. DOI: 10.1177/027836499301200102
14. Olfati-Saber R. Distributed Kalman filtering for sensor networks // 2007 46th IEEE Conference on Decision and Control. New Orleans, LA, USA, 2007. P. 5492-5498. DOI: 10.1109/CDC.2007.4434303
15. Roy S., Hashemi R.H., Laub A.J. Square root parallel Kalman filtering using reduced-order local filters // IEEE Transactions on Aerospace and Electronic Systems. 1991. Vol. 27, no. 2. P. 276-289. DOI: 10.1109/7.78303
16. Brown R.G., Hwang P.Y.C. Introduction to Random Signals and Applied Kalman Filtering: with MATLAB Exercises. 4th ed. Wiley, 1996.
17. Бортовая реализация адаптивно-робастных оценивающих фильтров: практические результаты / В.Л. Будкин, С.Л. Булгаков, Ю.П. Михеенков и др. // Научный вестник МГТУ ГА. 2005. № 89 (7). С. 59-71.
18. Семушин И.В., Цыганова Ю.В., Захаров К.В. Устойчивые алгоритмы фильтрации - обзор и новые результаты для систем судовождения // Информационные технологии и вычислительные системы. 2013. № 4. С. 90-112.
19. Thornton C.L. Triangular Covariance Factorizations for Kalman Filtering. Ph.D. Thesis. University of California at Los Angeles, 1976.
20. Complete solution classification for the perspective-three-point problem / X.-S. Gao, X.-R. Hou, J. Tang, H.-F. Cheng // IEEE Transactions on Pattern Analysis and Machine Intelligence. 2003. Vol. 25, no. 8. P. 930-943. DOI: 10.1109/TPAMI.2003.1217599
References
1. Bloesch M., Burri M., Omari S., Hutter M., Siegwart R. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback. The International Journal of Robotics Research. 2017;36(10): 1053-1072. DOI: 10.1177/0278364917728574
2. Li M., Mourikis A.I. Improving the accuracy of EKF-based visual-inertial odometry. In: 2012 IEEE International Conference on Robotics and Automation. Saint Paul, MN, USA; 2012. P. 828-835. DOI: 10.1109/ICRA.2012.6225229
3. Fang Q., Huang S.X. UKF for Integrated Vision and Inertial Sensors Based on Three-View Geometry. IEEE Sensors Journal. 2013;13(7):2711-2719. DOI: 10.1109/jsen.2013.2259228
4. Bleser G., Strickery D. Using the marginalised particle filter for real-time visual-inertial sensor fusion. In: 2008 7th IEEE/ACM International Symposium on Mixed and Augmented Reality. Cambridge, UK; 2008. P. 3-12. DOI: 10.1109/ISMAR.2008.4637316
5. Hong K., Kim S., Park J., Bang H. Particle filter approach to vision-based navigation with aerial image segmentation. Journal of Aerospace Information Systems. 2021;18(12):1-9. DOI: 10.2514/1.I010957
6. Gui J., Gu D., Wang S., Hu H. A review of visual inertial odometry from filtering and optimisation perspectives. Advanced Robotics. 2015;29(20):1289-1301. DOI: 10.1080/01691864.2015.1057616
7. Huang G. Visual-Inertial Navigation: A Concise Review. In: 2019 International Conference on Robotics and Automation (ICRA). Montreal, QC, Canada; 2019. P. 9572-9582. DOI: 10.1109/ICRA.2019.8793604
8. Tsyganova Yu.V., Kulikova M.V. On modern array algorithms for optimal discrete filtering. Bulletin of the South Ural State University. Ser. Mathematical Modelling, Programming & Computer Software. 2018;11(4):5-30. (In Russ.) DOI: 10.14529/mmp180401
9. Mourikis A.I., Roumeliotis S.I. A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. In: Proceedings 2007 IEEE International Conference on Robotics and Automation. Rome, Italy; 2007. P. 3565-3572. DOI: 10.1109/ROBOT.2007.364024
10. Won D.H., Chun S., Sung S., Lee Y., Cho J., Joo J., Park J. INS/vSLAM system using distributed particle filter. International Journal of Control Automation and Systems. 2010;8:1232-1240. DOI: 10.1007/s12555-010-0608-7
11. Paik B.S., Oh J.H. Gain fusion algorithm for decentralised parallel Kalman filters. IEE Proceedings - Control Theory and Applications. 2000;147(1):97-103. DOI: 10.1049/ip-cta:20000141
12. Hashemipour H.R., Roy S., Laub A.J. Decentralized structures for parallel Kalman filtering. IEEE Transactions on Automatic Control. 1988;33(1):88-94. DOI: 10.1109/9.364
13. Rao B.S.Y, Durrant-Whyte H.F., Sheen J.A. A Fully Decentralized Multi-Sensor System for Tracking and Surveillance. The International Journal of Robotics Research. 1993;12(1):20-44. DOI: 10.1177/027836499301200102
14. Olfati-Saber R. Distributed Kalman filtering for sensor networks. In: 2007 46th IEEE Conference on Decision and Control. New Orleans, LA, USA; 2007. P. 5492-5498. DOI: 10.1109/CDC.2007.4434303
15. Roy S., Hashemi R.H., Laub A.J. Square root parallel Kalman filtering using reduced-order local filters. IEEE Transactions on Aerospace and Electronic Systems. 1991;27(2):276-289. DOI: 10.1109/7.78303
16. Brown R.G., Hwang P.Y.C. Introduction to Random Signals and Applied Kalman Filtering: with MATLAB Exercises. 4th ed. Wiley; 1996.
17. Budkin V.L., Bulgakov S.L., Mikheenkov Yu.P., Chernodarov A.V., Patrikeev A.P. [On-board implementation of adaptive-robust estimation filters: practical results]. Nauchnyi Vestnik MGTU GA. 2005;89(7):59-71. (In Russ.)
18. Semushin I.V., Tsyganova Ju.V., Zakharov K.V. Robust filter algorithms - survey and new results for ship navigation and conning systems. Journal of Information Technologies and Computation Systems. 2013;(4):90-112. (In Russ.)
19. Thornton C.L. Triangular Covariance Factorizations for Kalman Filtering. Ph.D. Thesis. University of California at Los Angeles; 1976.
20. Gao X.-S., Hou X.-R., Tang J., Cheng H.-F. Complete solution classification for the perspective-three-point problem. IEEE Transactions on Pattern Analysis and Machine Intelligence. 2003;25(8):930-943. DOI: 10.1109/TPAMI.2003.1217599
Информация об авторе
Циоплиакис Николаос Илиас, аспирант кафедры информационно-измерительной техники, Южно-Уральский государственный университет, Челябинск, Россия; [email protected]. Information about the author
Nikolaos I. Tsiopliakis, Postgraduate student of the Department of Information and Measuring Technology, South Ural State University, Chelyabinsk, Russia; [email protected].
Статья поступила в редакцию 01.08.2024 The article was submitted 01.08.2024
