Moving Horizon Estimation for Simultaneous Localization and Mapping with Robust Estimation Error Bounds

TL;DR

This paper introduces a robust moving horizon estimation method for SLAM that guarantees bounded errors and stability, even with limited landmark visibility, by decoupling ego-state and landmark updates for improved efficiency.

Contribution

It proposes a novel decoupled MHE framework with provable error bounds and robustness for SLAM, accommodating limited landmark visibility and enabling parallel landmark updates.

Findings

The method guarantees estimation stability under certain detectability conditions.

Decoupling updates improves computational efficiency and robustness.

Simulation results demonstrate effectiveness under noisy conditions.

Abstract

This paper presents a robust moving horizon estimation (MHE) approach with provable estimation error bounds for solving the simultaneous localization and mapping (SLAM) problem. We derive sufficient conditions to guarantee robust stability in ego-state estimates and bounded errors in landmark position estimates, even under limited landmark visibility which directly affects overall system detectability. This is achieved by decoupling the MHE updates for the ego-state and landmark positions, enabling individual landmark updates only when the required detectability conditions are met. The decoupled MHE structure also allows for parallelization of landmark updates, improving computational efficiency. We discuss the key assumptions, including ego-state detectability and Lipschitz continuity of the landmark measurement model, with respect to typical SLAM sensor configurations, and introduce a streamlined method for the range measurement model. Simulation results validate the considered method, highlighting its efficacy and robustness to noise.