Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem

A new version of the extended Kalman filter (EKF) is proposed for nonlinear systems possessing symmetries. Instead of using a linear correction term based on a linear output error, it uses a geometrically adapted correction term based on an invariant output error; in the same way the gain matrix is not updated from of a linear state error, but from an invariant state error. The benefit is that the gain and covariance equations converge to constant values on a much bigger set of trajectories than equilibrium points as is the case for the EKF, which should result in a better convergence of the estimation. This filter is applied to the practically relevant problem of estimating the velocity and attitude of a moving rigid body, e.g. an aircraft, from GPS velocity, inertial and magnetic measurements. In this context it can be seen as an extension of the ¿multiplicative EKF¿ often used for quaternion estimation.