Invariant extended Kalman filter

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.

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}$

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

${\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[1]

${\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.

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[1] 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.[3]