Invariant extended Kalman filter

The invariant extended Kalman filter (IEKF) (not to be confused with the iterated extended Kalman filter) was first introduced as a version of the extended Kalman filter (EKF) for nonlinear systems possessing symmetries (or invariances), ^[1] then generalized and recast as an adaptation to Lie groups of the linear Kalman filtering theory. ^[2] Instead of using a linear correction term based on a linear output error, the IEKF uses a geometrically adapted correction term based on an invariant output error; in the same way the gain matrix is not updated from a linear state error, but from an invariant state error. The main benefit is that the gain and covariance equations have reduced dependence on the estimated value of the state. In some cases they converge to constant values on a much bigger set of trajectories than is the case for the EKF, which results in a better convergence of the estimation.

Filter derivation

Discrete-time framework

Consider a system whose state is encoded at time step ${\displaystyle n}$ by an element ${\displaystyle x_{n}}$ of a Lie group ${\displaystyle G}$ and dynamics has the following shape: ^[3]

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

where ${\displaystyle \phi _{n}}$ is a group automorphism of ${\displaystyle G}$, ${\displaystyle \cdot }$ is the group operation and ${\displaystyle u_{n}}$ an element of ${\displaystyle G}$. The system is supposed to be observed through a measurement ${\displaystyle y_{n}}$ having the following shape:

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

where ${\displaystyle b_{n}}$ belongs to a vector space ${\displaystyle {\mathcal {Y}}}$ endowed with a left action of the elements of ${\displaystyle G}$ denoted again by ${\displaystyle \cdot }$ (which cannot create confusion with the group operation as the second member of the operation is an element of ${\displaystyle {\mathcal {Y}}}$, not ${\displaystyle G}$). Alternatively, the same theory applies to a measurement defined by a right action:

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

Filter equations

The invariant extended Kalman filter is an observer ${\displaystyle {\hat {x}}_{n}}$ defined by the following equations if the measurement function is a left action:

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

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

where ${\displaystyle \exp()}$ is the exponential map of ${\displaystyle G}$ and ${\displaystyle K_{n}}$ is a gain matrix to be tuned through a Riccati equation.

If measurement function is a right action then the update state is defined as:

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

Continuous-time framework

The discrete-time framework above was first introduced for continuous-time dynamics of the shape:

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

where the vector field ${\displaystyle f_{t}}$ verifies at any time ${\displaystyle t}$ the relation: ^[2]

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

where the identity element of the group is denoted by ${\displaystyle Id}$ and is used the short-hand notation ${\displaystyle gu=L_{g}(u)}$ (resp. ${\displaystyle ug=R_{g}(u)}$) for the left translation ${\displaystyle L_{g}:TG_{x}\rightarrow TG_{gx}}$ (resp. the right translation ${\displaystyle R_{g}:TG_{x}\rightarrow TG_{xg}}$) where ${\displaystyle TG_{x}}$ denots the tangent space to ${\displaystyle G}$ at ${\displaystyle x}$. It leads to more involved computations than the discrete-time framework, but properties are similar.

Main properties

The main benefit of invariant extended Kalman filtering is the behavior of the invariant error variable, whose definition depends on the type of measurement. For left actions we define a left-invariant error variable as:

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

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

while for right actions we define a right-invariant error variable as:

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

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

Indeed, replacing ${\displaystyle x_{n}}$, ${\displaystyle {\hat {x}}_{n|n-1}}$, ${\displaystyle {\hat {x}}_{n|n}}$ by their values we obtain for left actions, after some algebra:

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

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

and for right actions:

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

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

We see the estimated value of the state is not involved in the equation followed by the error variable, a property of linear Kalman filtering the classical extended Kalman filter does not share, but the similarity with the linear case actually goes much further. Let ${\displaystyle \xi _{n|n-1},\xi _{n|n}}$ be a linear version of the error variable defined by the identity:

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

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

Then, with ${\displaystyle F_{n}}$ defined by the Taylor expansion ${\displaystyle \xi _{n|n}=F_{n}\xi _{n|n-1}+\circ (||\xi _{n|n-1}||)}$ we actually have: ^[2]

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

In other words, there are no higher-order terms: the dynamics is linear for the error variable ${\displaystyle \xi _{n|n},\xi _{n|n-1}}$. This result and error dynamics independence are at the core of theoretical properties and practical performance of IEKF. ^[2]

Relation to symmetry-preserving observers

Most physical systems possess natural symmetries (or invariance), i.e. there exist transformations (e.g. rotations, translations, scalings) that leave the system unchanged. From mathematical and engineering viewpoint, it makes sense that a filter well-designed for the considered system should preserve the same invariance properties. The idea for the IEKF is a modification of the EKF equations to take advantage of the symmetries of the system.

Definition

Consider the system

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

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

where ${\displaystyle w,v}$ are independent white Gaussian noises. Consider ${\displaystyle G}$ a Lie group with identity ${\displaystyle e}$, and (local) transformation groups ${\displaystyle \varphi _{g},\psi _{g},\rho _{g}}$ (${\displaystyle g\in G}$) such that ${\displaystyle (X,U,Y)=(\varphi _{g}(x),\psi _{g}(u),\rho _{g}(y))}$. The previous system with noise is said to be invariant if it is left unchanged by the action the transformations groups ${\displaystyle \varphi _{g},\psi _{g},\rho _{g}}$; that is, if

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

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

Filter equations and main result

Since it is a symmetry-preserving filter, the general form of an IEKF reads ^[4]

${\displaystyle {\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)}$

where

• ${\displaystyle E({\hat {x}},u,y)}$ is an invariant output error, which is different from the usual output error ${\displaystyle {\hat {y}}-y}$
• ${\displaystyle W({\hat {x}})={\bigl (}w_{1}({\hat {x}}),..,w_{n}({\hat {x}}){\bigr )}}$ is an invariant frame
• ${\displaystyle I({\hat {x}},u)}$ is an invariant vector
• ${\displaystyle L(I,E)}$ is a freely chosen gain matrix.

To analyze the error convergence, an invariant state error ${\displaystyle \eta ({\hat {x}},x)}$ is defined, which is different from the standard output error ${\displaystyle {\hat {x}}-x}$, since the standard output error usually does not preserve the symmetries of the system.

Given the considered system and associated transformation group, there exists a constructive method to determine ${\displaystyle E({\hat {x}},u,y),W({\hat {x}}),I({\hat {x}},u),\eta ({\hat {x}},x)}$, based on the moving frame method.

Similarly to the EKF, the gain matrix ${\displaystyle L(I,E)}$ is determined from the equations ^[5]

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

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

where the matrices ${\displaystyle A,C}$ depend here only on the known invariant vector ${\displaystyle I({\hat {x}},u)}$, rather than on ${\displaystyle ({\hat {x}},u)}$ as in the standard EKF. This much simpler dependence and its consequences are the main interests of the IEKF. Indeed, the matrices ${\displaystyle A,C}$ are then constant on a much bigger set of trajectories (so-called permanent trajectories) than equilibrium points as it is the case for the EKF. Near such trajectories, we are back to the "true", i.e. linear, Kalman filter where convergence is guaranteed. Informally, this means the IEKF converges in general at least around any slowly varying permanent trajectory, rather than just around any slowly varying equilibrium point for the EKF.

Application examples

Attitude and heading reference systems

Invariant extended Kalman filters are for instance used in attitude and heading reference systems. In such systems the orientation, velocity and/or position of a moving rigid body, e.g. an aircraft, are estimated from different embedded sensors, such as inertial sensors, magnetometers, GPS or sonars. The use of an IEKF naturally leads ^[5] to consider the quaternion error ${\displaystyle {\hat {q}}q^{-1}}$, which is often used as an ad hoc trick to preserve the constraints of the quaternion group. The benefits of the IEKF compared to the EKF are experimentally shown for a large set of trajectories. ^[6]

Inertial navigation

A major application of the Invariant extended Kalman filter is inertial navigation, which fits the framework after embedding of the state (consisting of attitude matrix ${\displaystyle R}$, velocity vector ${\displaystyle v}$ and position vector ${\displaystyle x}$) into the Lie group ${\displaystyle SE_{2}(3)}$ ^[7] defined by the group operation:

${\displaystyle (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})}$

Simultaneous localization and mapping

The problem of simultaneous localization and mapping also fits the framework of invariant extended Kalman filtering after embedding of the state (consisting of attitude matrix ${\displaystyle R}$, position vector ${\displaystyle x}$ and a sequence of static feature points ${\displaystyle p^{1},\dots ,p^{K}}$) into the Lie group ${\displaystyle SE_{K+1}(3)}$ (or ${\displaystyle SE_{K+1}(2)}$ for planar systems) ^[7] defined by the group operation:

${\displaystyle (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})}$

The main benefit of the Invariant extended Kalman filter in this case is solving the problem of false observability. ^[7]

References

1. Bonnabel, S. (2007, December). Left-invariant extended Kalman filter and attitude estimation. In 2007 46th IEEE Conference on Decision and Control (pp. 1027-1032). IEEE.
2. Barrau, A., & Bonnabel, S. (2016). The invariant extended Kalman filter as a stable observer. IEEE Transactions on Automatic Control, 62(4), 1797-1812.
3. Barrau, A., & Bonnabel, S. (2019). Linear observed systems on groups. Systems & Control Letters, 129, 36-42.
4. S. Bonnabel, Ph. Martin, and P. Rouchon, “Symmetry-preserving observers,” IEEE Transactions on Automatic and Control, vol. 53, no. 11, pp. 2514–2526, 2008.
5. S. Bonnabel, Ph. Martin and E. Salaün, "Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem", 48th IEEE Conference on Decision and Control, pp. 1297–1304, 2009.
6. Ph. Martin and E. Salaün, "Generalized Multiplicative Extended Kalman Filter for Aided Attitude and Heading Reference System", AIAA Guidance, Navigation and Control Conference, 2010
7. Barrau, A., & Bonnabel, S. (2015). An EKF-SLAM algorithm with consistency properties. arXiv preprint arXiv : 1510.06263.