🤖 AI Summary
This work addresses the degraded estimation performance in 3D landmark-aided inertial navigation caused by spurious observability in conventional filters. It introduces, for the first time, the Iterated Invariant Extended Kalman Filter (Iterated Invariant EKF) to this setting. By constraining the state to the observation manifold under low-noise conditions and restricting uncertainty to its tangent space, the method effectively preserves the system’s inherent observability structure. The approach integrates Lie group geometry, group-affine system modeling, and a 3D landmark observation model. Numerical simulations demonstrate that it significantly outperforms classical SO(3)-EKF, iterated SO(3)-EKF, and standard IEKF, achieving notable improvements in both estimation accuracy and consistency.
📝 Abstract
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.