Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Equivariant Filter Design for Range-only SLAM

Yixiao Ge, Arthur Pearce, Pieter van Goor, Robert Mahony

TL;DR

This work tackles range-only SLAM under no prior landmark information by formulating RO-SLAM as a problem on a Lie-group symmetry. The authors develop an equivariant filter (EqF) on the group $\mathbf{SLAM}^R_n(3)=\mathbf{SE}_2(3)\times\mathbf{SOT}(3)^n$, using $\,\mathbf{SE}_2(3)$ for the navigation state and $\,\mathbf{SOT}(3)$ normal coordinates for landmarks, with an equivariant output approximation and a lift of the dynamics. They prove symmetry properties, specify origin and local coordinates, and implement the EqF with Riccati-based updates and a reset step, validating it on real outdoor RO-SLAM data. Results show that EqF outperforms a standard EKF in path and landmark accuracy, achieving rapid convergence without landmark bootstrapping and remaining robust to measurement dropout, with practical implications for UWB/BLE and submarine beacon localization.

Abstract

Range-only Simultaneous Localisation and Mapping (RO-SLAM) is of interest due to its practical applications in ultra-wideband (UWB) and Bluetooth Low Energy (BLE) localisation in terrestrial and aerial applications and acoustic beacon localisation in submarine applications. In this work, we consider a mobile robot equipped with an inertial measurement unit (IMU) and a range sensor that measures distances to a collection of fixed landmarks. We derive an equivariant filter (EqF) for the RO-SLAM problem based on a symmetry Lie group that is compatible with the range measurements. The proposed filter does not require bootstrapping or initialisation of landmark positions, and demonstrates robustness to the no-prior situation. The filter is demonstrated on a real-world dataset, and it is shown to significantly outperform a state-of-the-art EKF alternative in terms of both accuracy and robustness.

Equivariant Filter Design for Range-only SLAM

TL;DR

This work tackles range-only SLAM under no prior landmark information by formulating RO-SLAM as a problem on a Lie-group symmetry. The authors develop an equivariant filter (EqF) on the group , using for the navigation state and normal coordinates for landmarks, with an equivariant output approximation and a lift of the dynamics. They prove symmetry properties, specify origin and local coordinates, and implement the EqF with Riccati-based updates and a reset step, validating it on real outdoor RO-SLAM data. Results show that EqF outperforms a standard EKF in path and landmark accuracy, achieving rapid convergence without landmark bootstrapping and remaining robust to measurement dropout, with practical implications for UWB/BLE and submarine beacon localization.

Abstract

Range-only Simultaneous Localisation and Mapping (RO-SLAM) is of interest due to its practical applications in ultra-wideband (UWB) and Bluetooth Low Energy (BLE) localisation in terrestrial and aerial applications and acoustic beacon localisation in submarine applications. In this work, we consider a mobile robot equipped with an inertial measurement unit (IMU) and a range sensor that measures distances to a collection of fixed landmarks. We derive an equivariant filter (EqF) for the RO-SLAM problem based on a symmetry Lie group that is compatible with the range measurements. The proposed filter does not require bootstrapping or initialisation of landmark positions, and demonstrates robustness to the no-prior situation. The filter is demonstrated on a real-world dataset, and it is shown to significantly outperform a state-of-the-art EKF alternative in terms of both accuracy and robustness.

Paper Structure

This paper contains 18 sections, 3 theorems, 29 equations, 3 figures, 2 tables.

Table of Contents

  1. INTRODUCTION
  2. PRELIMINARY
  3. Relevant Lie groups
  4. PROBLEM DESCRIPTION
  5. Ego-state reformulation
  6. EQUIVARIANT FILTER DESIGN
  7. Symmetry of RO-SLAM
  8. Origin choice and local coordinates
  9. EqF Dynamics
  10. EXPERIMENTS
  11. Hardware Setup
  12. Filter Implementation
  13. Results
  14. CONCLUSION
  15. Symmetry group proofs
  16. ...and 3 more sections

Key Result

Lemma 4.1

The group action $\phi:\mathbf{SLAM}^R_n(3)\times\mathcal{M}\to\mathcal{M}$ defined by is a transitive right group action.

Figures (3)

  • Figure 1: Visualisation of the selected vehicle trajectories (Mission 1 UAV 1, Mission 2 UGV 1) and the landmark positions.
  • Figure 2: The true and estimated trajectories of the robot and the landmark positions after alignment. The axes are adjusted for better visualisation.
  • Figure 3: Estimation error of landmark positions on Mission 1 UAV 1.

Theorems & Definitions (4)

  • Remark 3.1
  • Lemma 4.1: van2019geometric
  • Lemma 4.2
  • Lemma 4.3