Pi-Invariant Unscented Kalman Filter for sensor fusion

A novel approach based on Unscented Kalman Filter (UKF) is proposed for nonlinear state estimation. The Invariant UKF, named π-IUKF, is a recently introduced algorithm dedicated to nonlinear systems possessing symmetries as illustrated by the quaternion-based mini Remotely Piloted Aircraft System (RPAS) kinematics modeling considered in this paper. Within an invariant framework, this algorithm suggests a systematic approach to determine all the symmetry-preserving terms which correct accordingly the nonlinear state-space representation used for prediction, without requiring any linearization. Thus, based on both invariant filters, for which Lie groups have been identified and UKF theoretical principles, the developed π-IUKF has been previously and successfully applied to the mini-RPAS attitude estimation problem, highlighting remarkable invariant properties. We propose in this paper to extend the theoretical background and the applicability of our proposed π-IUKF observer to the case of a mini-RPAS equipped with an aided Inertial Navigation System (INS) which leads to augment the nonlinear state space representation with both velocity and position differential equations. All the measurements are provided on board by a set of low-cost and low-performance sensors (accelerometers, gyrometers, magnetometers, barometer and even Global Positioning System (GPS)). Our designed π-IUKF estimation algorithm is described in this paper and its performances are evaluated by exploiting successfully real flight test data. Indeed, the whole approach has been implemented onboard using a data logger based on the well-known Paparazzi system. The results show promising perspectives and demonstrate that nonlinear state estimation converges on a much bigger set of trajectories than for more traditional approaches