Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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

Jelena Trisovic, Alexandre Didier, Simon Muntwiler, Melanie N. Zeilinger

TL;DR

A robust moving horizon estimation approach with provable estimation error bounds for solving the simultaneous localization and mapping (SLAM) problem is presented and the key assumptions, including ego-state detectability and Lipschitz continuity of the landmark measurement model, are discussed.

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.

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

TL;DR

A robust moving horizon estimation approach with provable estimation error bounds for solving the simultaneous localization and mapping (SLAM) problem is presented and the key assumptions, including ego-state detectability and Lipschitz continuity of the landmark measurement model, are discussed.

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.

Paper Structure

This paper contains 10 sections, 2 theorems, 27 equations, 3 figures, 1 algorithm.

Table of Contents

  1. Introduction
  2. Preliminaries
  3. Problem Formulation
  4. Decoupled MHE for SLAM
  5. Discussion
  6. Common SLAM Configurations
  7. Verification of Assumptions
  8. Streamlined Method for the Range Measurement Model
  9. Simulation Results
  10. Conclusion

Key Result

Theorem 1

(Corollary 1 from schiller2023lyapunov) Let system eq:gen_sys be detectable, i.e., admit an i-IOSS Lyapunov function according to Definition def1. Then the MHE scheme eq:MHEgeneral with horizon $M$ chosen such that $4\eta^M\lambda_{\max}(\underline{U}, \overline{U}) < 1$ is RGES according to Definit

Figures (3)

  • Figure 2: Planar robot and four landmarks. Coordinates of the robot are denoted by $x^s=(p_x^{\top}, p_y^{\top}, \theta^{\top})^{\top}$, and the coordinates of each landmark $l$ with $x^{e,l}=(p_x^{e,l\top}, p_y^{e,l \top})^{\top}$. Measurement of landmark $l=3$ coming from a bearing-only sensor is a unit vector denoted by $y_b^{e,3}$, while the measurement obtained with a range sensor is denoted as $y_r^{e,3}$.
  • Figure 3: Circular (left) and corridor (right) landmark placement (indicated by stars) and corresponding system trajectories.
  • Figure 4: Ego-state (top) and average landmark error (bottom) for the circular (left) and corridor (right) scenarios.

Theorems & Definitions (6)

  • Definition 1
  • Definition 2
  • Theorem 1
  • Definition 3
  • Proposition 1
  • Remark 1