Track papers across labs, topics, and trends.
Search papers, labs, and topics across Lattice.
This paper introduces the Iterated Invariant Extended Kalman Filter (IterIEKF) for landmark-aided inertial navigation, addressing the limitations of classical SO(3)-EKF methods that suffer from overconfidence in unobservable directions. By reformulating the system dynamics as a group-affine system on a Lie group and ensuring state compatibility, the IterIEKF maintains the estimated state on the observed state manifold while confining uncertainty to its tangent space. Numerical simulations demonstrate that the IterIEKF significantly outperforms existing methods in estimation accuracy and consistency, marking a notable advancement in 3D localization techniques.
The Iterated Invariant EKF achieves superior estimation accuracy and consistency in 3D localization, outperforming traditional methods that struggle with unobservable directions.
Inertial navigation systems aided by three-dimensional landmark measurements constitute a fundamental problem in robotic perception and state estimation. Classical SO(3)-based Extended Kalman Filter (SO(3)-EKF) approaches provide practical solutions, but suffer from the false observability problem, in which the filter becomes overconfident in unobservable directions, leading to degraded estimation performance. The Invariant EKF (IEKF) addresses this limitation by reformulating the system dynamics as a group-affine system on a Lie group, although its measurement update does not fully satisfy certain state compatibility properties. More recently, the Iterated Invariant EKF (IterIEKF) was proposed to further improve the IEKF by ensuring, in the low-noise regime, that the estimated state remains on the observed state manifold while the uncertainty is confined to its tangent space. In this work, we formulate and apply the IterIEKF to landmark-based inertial 3D localization for the first time. Through numerical simulations, we show that the proposed approach outperforms the classical SO(3)-EKF, the Iterated SO(3)-EKF, and the IEKF in terms of both estimation accuracy and consistency.