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

TL;DR

This paper presents a novel manifold-based error state Kalman filter for ground vehicle localization that improves long-term consistency and accuracy by incorporating terrain geometry and domain knowledge, outperforming classical methods.

Contribution

It introduces the Manifold Error State Extended Kalman Filter (M-ESEKF), a new formulation that reduces pose estimation errors on smooth surfaces without artificial constraints.

Findings

Outperforms classical filters in consistency and stability.

Eliminates scenario-specific parameter tuning.

Compatible with diverse sensor configurations.

Abstract

Aiming to enhance the consistency and thus long-term accuracy of Extended Kalman Filters for terrestrial vehicle localization, this paper introduces the Manifold Error State Extended Kalman Filter (M-ESEKF). By representing the robot's pose in a space with reduced dimensionality, the approach ensures feasible estimates on generic smooth surfaces, without introducing artificial constraints or simplifications that may degrade a filter's performance. The accompanying measurement models are compatible with common loosely- and tightly-coupled sensor modalities and also implicitly account for the ground geometry. We extend the formulation by introducing a novel correction scheme that embeds additional domain knowledge into the sensor data, giving more accurate uncertainty approximations and further enhancing filter consistency. The proposed estimator is seamlessly integrated into a validated modular state estimation framework, demonstrating compatibility with existing implementations. Extensive Monte Carlo simulations across diverse scenarios and dynamic sensor configurations show that the M-ESEKF outperforms classical filter formulations in terms of consistency and stability. Moreover, it eliminates the need for scenario-specific parameter tuning, enabling its application in a variety of real-world settings.