Multi-Modal Tightly Coupled Robust Pose Estimation for Mobile Robots in Complex Degraded Scenarios

Current multi-modal pose estimation methods often suffer from severe localization divergence and prohibitive computational overhead when confronted with extreme scenarios such as sudden illumination variations, geometric degeneracy, and wheel slippage. To address these critical challenges, this paper presents a tightly coupled multi-modal pose estimation algorithm for mobile robots utilizing adaptive robust manifold filtering. First, a pre-integration-driven iterated error-state Kalman filter (iESKF) is formulated on the Lie group manifold to eliminate redundant re-integration workloads. Second, the Mahalanobis distance chi-square test and M-estimation are introduced to adaptively isolate non-Gaussian heavy-tailed noise caused by perception degradation. Finally, a perception health quantification system and a smooth degradation state machine are designed to handle concurrent perceptual blindness and wheel slippage. Experimental results demonstrate that the algorithm takes an average of only 12.8 ms per frame on edge computing platforms. Under severely degraded and composite environments, the algorithm limits the typical end-to-end closed-loop drift to 1.37 m (with a statistical average of 1.24 m) over a 100-m trajectory, translating to a relative translation error (RTE) of approximately 1.2% to 1.4%. This demonstrates an exceptional balance between high real-time efficiency and robust survivability.