The invariant extended Kalman filter (IEKF) (not to be confused with the iterated extended Kalman filter) is a new version of the extended Kalman filter (EKF) for nonlinear systems possessing symmetries (or invariances). It combines the advantages of both the EKF and the recently introduced symmetry-preserving filters. 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 converge to constant values on a much bigger set of trajectories than equilibrium points that is the case for the EKF, which results in a better convergence of the estimation.
Contents
Motivation
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
where
Filter equations and main result
Since it is a symmetry-preserving filter, the general form of an IEKF reads
where
To analyze the error convergence, an invariant state error
Given the considered system and associated transformation group, there exists a constructive method to determine
Similarly to the EKF, the gain matrix
where the matrices
Application example in aerospace engineering
Invariant extended Kaman 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 to consider the quaternion error