Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Nonlinear Observer Design for Landmark-Inertial Simultaneous Localization and Mapping

Mouaad Boughellaba, Soulaimane Berkane, Abdelhamid Tayebi

TL;DR

The paper develops a nonlinear geometric observer for landmark-inertial SLAM by embedding the system in the augmented Lie group $SE_{3+n}(3)$ and fusing IMU data with landmark measurements. A key idea is introducing an auxiliary state to decouple gravity-induced coupling, enabling an almost global asymptotic stability (AGAS) result while estimating pose and landmarks up to an unknown constant yaw. The observer employs a left-invariant error framework and a residual-based innovation that leads to a linear-in-$n$ error dynamics with a design that guarantees AGAS for the full system. Numerical simulations with multiple landmarks demonstrate robust SLAM performance and favorable real-time scalability compared to EKF-based methods.

Abstract

This paper addresses the problem of Simultaneous Localization and Mapping (SLAM) for rigid body systems in three-dimensional space. We introduce a new matrix Lie group SE_{3+n}(3), whose elements are composed of the pose, gravity, linear velocity and landmark positions, and propose an almost globally asymptotically stable nonlinear geometric observer that integrates Inertial Measurement Unit (IMU) data with landmark measurements. The proposed observer estimates the pose and map up to a constant position and a constant rotation about the gravity direction. Numerical simulations are provided to validate the performance and effectiveness of the proposed observer, demonstrating its potential for robust SLAM applications.

Nonlinear Observer Design for Landmark-Inertial Simultaneous Localization and Mapping

TL;DR

The paper develops a nonlinear geometric observer for landmark-inertial SLAM by embedding the system in the augmented Lie group and fusing IMU data with landmark measurements. A key idea is introducing an auxiliary state to decouple gravity-induced coupling, enabling an almost global asymptotic stability (AGAS) result while estimating pose and landmarks up to an unknown constant yaw. The observer employs a left-invariant error framework and a residual-based innovation that leads to a linear-in- error dynamics with a design that guarantees AGAS for the full system. Numerical simulations with multiple landmarks demonstrate robust SLAM performance and favorable real-time scalability compared to EKF-based methods.

Abstract

This paper addresses the problem of Simultaneous Localization and Mapping (SLAM) for rigid body systems in three-dimensional space. We introduce a new matrix Lie group SE_{3+n}(3), whose elements are composed of the pose, gravity, linear velocity and landmark positions, and propose an almost globally asymptotically stable nonlinear geometric observer that integrates Inertial Measurement Unit (IMU) data with landmark measurements. The proposed observer estimates the pose and map up to a constant position and a constant rotation about the gravity direction. Numerical simulations are provided to validate the performance and effectiveness of the proposed observer, demonstrating its potential for robust SLAM applications.

Paper Structure

This paper contains 7 sections, 4 theorems, 25 equations, 3 figures.

Table of Contents

  1. Introduction
  2. Preliminaries
  3. Problem Statement
  4. Observer Design
  5. SIMULATION
  6. CONCLUSIONS
  7. Proof of Theorem 1

Key Result

Lemma 1

The pair $(A,C)$ in lit_cls is Kalman observable, i.e., there exists a matrix gain $L$ such $(A-LC)$ is Hurwitz and the equilibrium point $x=0$ for the closed-loop system lit_cls is globally exponentially stable.

Figures (3)

  • Figure 1: Structure of the proposed SLAM observer.
  • Figure 2: Estimation errors of rotation, position, velocity and landmarks considering the constant transformation $(R^*, p^*)$
  • Figure 3: Estimated trajectories in the presence of noisy measurements and considering the constant transformation $(R^*, p^*)$. To avoid cluttering the figure, we have omitted the initial portions of the estimated landmark trajectories (blue dashed lines).

Theorems & Definitions (8)

  • Lemma 1
  • proof
  • Theorem 1
  • proof
  • Lemma 2
  • proof
  • Lemma 3
  • proof