Инвариантный расширенный фильтр Калмана

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

Вывод фильтра

Дискретное время

Рассмотрим систему, состояние которой кодируется на временном шаге $n$ по элементу $x_{n}$ Ли группы $G$ а динамика имеет следующий вид:

$x_{n}=\phi _{n}(x_{n-1})\cdot u_{n}$

где $\phi _{n}$ является групповым автоморфизмом $G$, $\cdot$ групповая операция и $u_{n}$ элемент $G$. Предполагается, что систему следует наблюдать посредством измерений. $y_{n}$ имеющий следующую форму:

$y_{n}=x_{n}\cdot b_{n}$

где $b_{n}$ принадлежит векторному пространству ${\mathcal {Y}}$ наделены левым действием элементов $G$ снова обозначается $\cdot$ (что не может привести к путанице с групповой операцией, поскольку второй член операции является элементом ${\mathcal {Y}}$, нет $G$). Альтернативно, та же теория применима к измерению, определяемому правильным действием :

$y_{n}=b_{n}\cdot x_{n}$

Уравнения фильтра

Инвариантный расширенный фильтр Калмана является наблюдателем ${\hat {x}}_{n}$ определяется следующими уравнениями, если функция измерения является левым действием:

${\hat {x}}_{n|n-1}=\phi _{n}({\hat {x}}_{n-1|n-1})\cdot u_{n}$

${\hat {x}}_{n|n}={\hat {x}}_{n|n-1}\cdot \exp(K_{n}({\hat {x}}_{n|n-1}^{-1}\cdot y_{n}-b_{n}))$

где $\exp()$ представляет собой экспоненциальную карту $G$ и $K_{n}$ — это матрица усиления, которую необходимо настроить с помощью уравнения Риккати.

Если функция измерения является правильным действием, то состояние обновления определяется как:

${\hat {x}}_{n|n}=\exp(K_{n}(y_{n}\cdot {\hat {x}}_{n|n-1}^{-1}-b_{n}))\cdot {\hat {x}}_{n|n-1}$

Непрерывная структура времени

Приведенная выше структура дискретного времени была впервые введена для динамики формы в непрерывном времени:

${\frac {d}{dt}}x_{t}=f_{t}(x_{t}),$

где векторное поле $f_{t}$ проверяет в любое время $t$ отношение:

$\forall a,b\in G,f_{t}(a\cdot b)=f_{t}(a)b+af_{t}(b)-af(Id)b$

где единичный элемент группы обозначается через $Id$ и используется сокращенное обозначение $gu=L_{g}(u)$ (соответственно $ug=R_{g}(u)$) для левого перевода $L_{g}:TG_{x}\rightarrow TG_{gx}$ (соответственно правильный перевод $R_{g}:TG_{x}\rightarrow TG_{xg}$) где $TG_{x}$ обозначает касательное пространство к $G$ в $x$. Это приводит к более сложным вычислениям, чем структура с дискретным временем, но свойства аналогичны.

Основные свойства

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

$e_{n|n-1}^{L}={\hat {x}}_{n|n-1}^{-1}x_{n}$,

$e_{n|n}^{L}={\hat {x}}_{n|n}^{-1}x_{n}$,

в то время как для правильных действий мы определяем правоинвариантную переменную ошибки как:

$e_{n|n-1}^{R}=x_{n}{\hat {x}}_{n|n-1}^{-1}$,

$e_{n|n}^{R}=x_{n}{\hat {x}}_{n|n}^{-1}$,

Действительно, замена $x_{n}$, ${\hat {x}}_{n|n-1}$, ${\hat {x}}_{n|n}$ по их значениям для левых действий после некоторой алгебры получаем:

$e_{n|n-1}^{L}=u_{n}^{-1}\cdot \phi _{n}(e_{n|n-1}^{L})\cdot u_{n}$,

$e_{n|n}^{L}=\exp(-K_{n}(e_{n|n-1}^{L}\cdot b_{n}-b_{n}))\cdot e_{n|n-1}^{L}$,

и для правильных действий:

$e_{n|n-1}^{R}=\phi _{n}(e_{n|n-1}^{R})$,

$e_{n|n}^{R}=e_{n|n-1}^{R}\exp(-K_{n}(b_{n}\cdot e_{n|n-1}^{R}-b_{n}))$,

Мы видим, что оценочное значение состояния не участвует в уравнении, за которым следует переменная ошибки, свойство линейной фильтрации Калмана, которым не обладает классический расширенный фильтр Калмана , но сходство с линейным случаем на самом деле идет гораздо дальше. Позволять $\xi _{n|n-1},\xi _{n|n}$ быть линейной версией переменной ошибки, определенной тождеством:

$e_{n|n-1}=\exp(\xi _{n|n-1}),$

$e_{n|n}=\exp(\xi _{n|n}).$

Затем с $F_{n}$ определяется расширением Тейлора $\xi _{n|n}=F_{n}\xi _{n|n-1}+\circ (||\xi _{n|n-1}||)$ на самом деле у нас есть:

$\xi _{n|n}=F_{n}\xi _{n|n-1}.$

Другими словами, членов высшего порядка нет: динамика линейна для переменной ошибки $\xi _{n|n},\xi _{n|n-1}$. Этот результат и независимость динамики ошибок лежат в основе теоретических свойств и практических характеристик IEKF.

Отношение к наблюдателям, сохраняющим симметрию

Большинство физических систем обладают естественной симметрией (или инвариантностью), т.е. существуют преобразования (например, вращения, перемещения, масштабирование), которые оставляют систему неизменной. С математической и инженерной точки зрения имеет смысл, что фильтр, хорошо спроектированный для рассматриваемой системы, должен сохранять те же свойства инвариантности. Идея IEKF заключается в модификации уравнений EKF, позволяющей воспользоваться преимуществами симметрии системы.

Определение

Рассмотрим систему

${\dot {x}}{=}f(x,u)+M(x)w$

$y{=}h(x,u)+N(x)v$

где $w,v$ являются независимыми белыми гауссовскими шумами .Учитывать $G$ группа Лжи с идентичностью $e$, и(локальные) группы преобразований $\varphi _{g},\psi _{g},\rho _{g}$ ( $g\in G$) такой, что $(X,U,Y)=(\varphi _{g}(x),\psi _{g}(u),\rho _{g}(y))$. Предыдущая система с шумом называется инвариантной, если она остается неизменной под действием групп преобразований $\varphi _{g},\psi _{g},\rho _{g}$; то есть, если

${\dot {X}}{=}f(X,U)+M(X)w$.

$Y{=}h(X,U)+N(X)v$

Уравнения фильтра и основной результат

Поскольку это фильтр, сохраняющий симметрию , общая форма IEKF гласит:

${\dot {\hat {x}}}=f({\hat {x}},u)+W({\hat {x}})L{\Bigl (}I({\hat {x}},u),E({\hat {x}},u,y){\Bigr )}E({\hat {x}},u,y)$

где

• $E({\hat {x}},u,y)$ — инвариантная ошибка вывода, отличающаяся от обычной ошибки вывода. ${\hat {y}}-y$
• $W({\hat {x}})={\bigl (}w_{1}({\hat {x}}),..,w_{n}({\hat {x}}){\bigr )}$ является инвариантным фреймом
• $I({\hat {x}},u)$ — инвариантный вектор
• $L(I,E)$ является свободно выбранной матрицей усиления.

Для анализа сходимости ошибок используется ошибка инвариантного состояния. $\eta ({\hat {x}},x)$ определяется, что отличается от стандартной ошибки вывода ${\hat {x}}-x$, поскольку стандартная ошибка вывода обычно не сохраняет симметрию системы.

Учитывая рассматриваемую систему и связанную с ней группу преобразований, существует конструктивный метод определения $E({\hat {x}},u,y),W({\hat {x}}),I({\hat {x}},u),\eta ({\hat {x}},x)$, основанный на методе движущейся рамки.

Как и в случае с EKF, матрица усиления $L(I,E)$ определяется из уравнений

$L{=}PC^{T}{\bigl (}N(e)N^{T}(e){\bigr )}^{-1}$,

${\dot {P}}{=}AP+PA^{T}+M(e)M^{T}(e)-PC^{T}{\bigl (}N(e)N^{T}(e){\bigr )}^{-1}CP$,

где матрицы $A,C$ зависят здесь только от известного инвариантного вектора $I({\hat {x}},u)$, а не на $({\hat {x}},u)$ как в стандартном EKF. Эта гораздо более простая зависимость и ее последствия являются основными интересами IEKF. Действительно, матрицы $A,C$ тогда они постоянны на гораздо большем наборе траекторий (так называемых постоянных траекториях ), чем точки равновесия, как в случае с EKF. Вблизи таких траекторий мы возвращаемся к «истинному», т. е. линейному фильтру Калмана, где сходимость гарантирована. Неформально это означает, что IEKF сходится в общем, по крайней мере, вокруг любой медленно меняющейся постоянной траектории, а не только вокруг любой медленно меняющейся точки равновесия для EKF.

Примеры применения

Системы ориентации и направления

Инвариантные расширенные фильтры Калмана используются, например, в системах ориентации и курса . В таких системах ориентация, скорость и/или положение движущегося твердого тела,например, самолет, оцениваются с помощью различных встроенных датчиков, таких как инерциальные датчики, магнитометры, GPS или гидролокаторы. Использование IEKF естественным образом приводит учитывать кватерниона ошибку ${\hat {q}}q^{-1}$, который часто используется как специальный трюк для сохранения ограничений группы кватернионов. Преимущества IEKF по сравнению с EKF экспериментально показаны для большого набора траекторий.

Инерциальная навигация

Основным применением Инвариантного расширенного фильтра Калмана является инерциальная навигация , которая соответствует структуре после внедрения состояния (состоящего из матрицы отношений $R$, вектор скорости $v$ и вектор положения $x$) в группу Лия $SE_{2}(3)$ определяется групповой операцией:

$(R_{1},v_{1},x_{1})\cdot (R_{2},v_{2},x_{2})=(R_{1}R_{2},x_{1}+R_{1}x_{2},v_{1}+R_{1}v_{2})$

Одновременная локализация и картографирование

Проблема одновременной локализации и отображения также укладывается в рамки инвариантной расширенной фильтрации Калмана после встраивания состояния (состоящего из матрицы отношений $R$, вектор положения $x$ и последовательность статических характерных точек $p^{1},\dots ,p^{K}$) в группу Лия $SE_{K+1}(3)$ (или $SE_{K+1}(2)$ для плоских систем) определяется групповой операцией:

$(R_{1},x_{1},p_{1}^{1},\dots ,p_{1}^{K})\cdot (R_{2},x_{2},p_{2}^{2},\dots ,p_{2}^{K})=(R_{1}R_{2},x_{1}+R_{1}x_{2},p_{1}^{1}+R_{1}p_{2}^{1},\dots ,p_{1}^{K}+R_{1}p_{2}^{K})$

Основным преимуществом Инвариантного расширенного фильтра Калмана в этом случае является решение проблемы ложной наблюдаемости.