Nonlinear Observer Design for Landmark-Inertial Simultaneous Localization and Mapping

2025-04-05

AI Summary

This paper addresses the joint estimation of orientation, gravity vector, linear velocity, and landmark positions in 3D rigid-body SLAM. We propose a geometric modeling framework based on the novel matrix Lie group $SE_{3+n}(3)$, enabling the first unified, integrated representation of these four state components. Building upon this, we design a nonlinear geometric observer with almost global asymptotic stability, which fuses IMU preintegration with robust landmark measurements—tolerant to outliers—while being inherently insensitive only to ambiguities in heading (rotation about the gravity axis) and global translation. Simulation results demonstrate that the method guarantees consistent convergence of pose and map estimates even under large initial errors, significantly improving geometric consistency and system robustness. It thus meets the stringent requirements of practical SLAM systems regarding both stability and accuracy.

Technology Category

• Intelligent Robots: State Estimation
• Search and Optimization: Sampling/Simulation-based Search
• Knowledge Representation and Reasoning: Geometric, Spatial, and Temporal Reasoning

Application Category

• Systems and Infrastructure for Web, Mobile and WoT: Location- and context-aware Web and WoT applications and services
• Semantics and Knowledge: Methods to enhance, augment, integrate or synergize semantic models such as knowledge graphs and LLMs
• Graph Algorithms and Modeling for the Web: Graph embeddings and representation learning for Web-related graphs

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.

Problem

• Designing a nonlinear observer for 3D SLAM using landmark-inertial data
• Estimating pose and map with constant position and rotation offsets
• Validating observer performance via simulations for robust SLAM applications

Innovation

• Introduces SE_{3+n}(3) matrix Lie group
• Proposes globally stable nonlinear observer
• Integrates IMU with landmark measurements