Magneto-visual-inertial Dead-reckoning: Improving Estimation Consistency by Invariance

Tractable algorithms used for 6DOF visual-inertial odometry have decades-long history of estimation consistency issues. Those arise in particular in two well-studied filters: namely the EKF-SLAM and MSCKF. Recently, strong theoretical works linked the error-state of these filters with their consistency properties; these results led to the synthesis of far more consistent filters. In previous works, we have shown that using similar filter for the fusion of magneto-inertial sensors with optical ones improved classical visual-inertial navigation systems. The consistency of those novel magneto-visual-inertial filters were, however, not addressed until now. This work does. We apply invariance theory findings to the specific case of magneto-inertial odometry and magneto-visual-inertial odometry for the synthesis of a filter with interesting consistency properties. We describe thoroughly such an invariant filter, implement it and conduct experiments on carefully captured data from real sensor. By comparing the results of non-invariant, observability-constrained and invariant versions of the filter, we find that the invariant version (i) shows an error estimate that is consistent with observability of the system, (ii) is applicable in case of unknown heading at initialization, (iii) improves long-term behavior of the filter and (iv) exhibits a lower normalized estimation error. We experiment on challenging scenarios for regular visual-inertial pedestrian navigation systems.