Invariant Extended Kalman Filter design for a magnetometer-plus-GPS aided inertial navigation system

We introduce a magnetometer-plus-GPS aided inertial navigation system for a helicopter UAV. A nonlinear observer is required to estimate the navigation states, typically an Extended Kalman Filter (EKF). A novel approach is the invariant observer, a constructive design method applicable to systems possessing symmetries. We review the theory and design an invariant observer for our example. Using an invariant observer guarantees a simplified form of the nonlinear estimation error dynamics. These are stabilized using an adaptation of the Invariant EKF, a systematic approach to compute the gains of an invariant observer. The resulting design is successfully implemented and validated in experiment and shows an improvement in performance over a conventional EKF.