Consistent Pose Estimation of Unmanned Ground Vehicles through Terrain-Aided Multi-Sensor Fusion on Geometric Manifolds

This article presents the Manifold Error State Extended Kalman Filter (M-ESEKF), a state-estimation method designed to improve long-term consistency and accuracy in terrestrial vehicle localization. By representing the vehicle’s pose on a lower-dimensional manifold, the approach naturally incorporates ground geometry and remains compatible with common sensing configurations. The filter includes a novel correction scheme that embeds domain knowledge into the measurement update, leading to more reliable uncertainty estimates and improved stability. Extensive Monte Carlo evaluations show that the M-ESEKF outperforms classical EKF formulations without the need for scenario-specific parameter tuning.