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

2025-08-20

AI Summary

To address poor filter consistency and long-term accuracy degradation in Extended Kalman Filter (EKF)-based pose estimation for ground unmanned vehicles operating on smooth terrain, this paper proposes a Manifold-based Error-State EKF (M-ESEKF). The method directly models vehicle pose on a low-dimensional geometric manifold, intrinsically embedding terrain priors to avoid ad hoc constraints or model simplifications. It fuses loosely and tightly coupled multi-sensor data, introduces a terrain-aided measurement model, and incorporates a novel uncertainty correction mechanism. Experimental results demonstrate that M-ESEKF significantly improves estimation consistency and long-term stability. Crucially, it achieves robust performance across diverse dynamic scenarios without scenario-specific parameter tuning, exhibiting strong generalization capability. The proposed framework establishes a principled, manifold-aware sensor fusion paradigm for robust localization in complex terrains.

Technology Category

• Intelligent Robots: State Estimation
• Machine Learning: Learning with Manifolds
• Search and Optimization: Mixed Discrete/Continuous Search

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Systems and Infrastructure for Web, Mobile and WoT: Location- and context-aware Web and WoT applications and services
• Security and Privacy: Large-scale security measurements

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.

Problem

• Enhancing consistent pose estimation for unmanned ground vehicles
• Improving Extended Kalman Filter accuracy through terrain-aided fusion
• Reducing dimensionality for feasible estimates on smooth surfaces

Innovation

• Manifold Error State Extended Kalman Filter
• Terrain-aided multi-sensor fusion
• Novel correction scheme embedding domain knowledge