Nonlinear Filter for Simultaneous Localization and Mapping on a Matrix Lie Group using IMU and Feature Measurements (2101.01648v2)

Abstract: Simultaneous Localization and Mapping (SLAM) is a process of concurrent estimation of the vehicle's pose and feature locations with respect to a frame of reference. This paper proposes a computationally cheap geometric nonlinear SLAM filter algorithm structured to mimic the nonlinear motion dynamics of the true SLAM problem posed on the matrix Lie group of $\mathbb{SLAM}_{n}\left(3\right)$. The nonlinear filter on manifold is proposed in continuous form and it utilizes available measurements obtained from group velocity vectors, feature measurements and an inertial measurement unit (IMU). The unknown bias attached to velocity measurements is successfully handled by the proposed estimator. Simulation results illustrate the robustness of the proposed filter in discrete form demonstrating its utility for the six-degrees-of-freedom (6 DoF) pose estimation as well as feature estimation in three-dimensional (3D) space. In addition, the quaternion representation of the nonlinear filter for SLAM is provided. Keywords: Simultaneous Localization and Mapping, Nonlinear observer algorithm for SLAM, inertial measurement unit, inertial vision system, pose, position, attitude, landmark, estimation, IMU, SE(3), SO(3), unmanned aerial vehicle, rigid-body, noise, nonlinear observer for SLAM, Gaussian filter, Kalman filtering, navigation.

• The paper introduces a novel geometric SLAM filter on matrix Lie groups that models the true nonlinear dynamics of 6-DoF spatial navigation.
• It leverages real-time IMU and feature measurements to effectively counteract sensor biases and improve pose and environmental feature estimations.
• Empirical results highlight improved noise robustness and computational efficiency, paving the way for advanced autonomous navigation applications.

Overview of a Nonlinear Filter for SLAM on a Matrix Lie Group

This paper presents an elaborate approach to Simultaneous Localization and Mapping (SLAM) by introducing a novel nonlinear filter operating on the matrix Lie group framework. This innovative design is structured to precisely mimic the nonlinear dynamics intrinsic to SLAM problems, particularly when they are placed onto the matrix Lie group $\mathbb{SLAM}_{n}(3)$. The authors leverage Inertial Measurement Units (IMUs) and feature measurements to improve both robustness and computational efficiency in estimating vehicle pose and feature locations concurrently.

Technical Approach and Methodological Design

The paper introduces a geometric nonlinear SLAM filter that capitalizes on the real-time data obtained from group velocity vectors, feature measurements, and IMU readings. Different from classical approaches relying heavily on Gaussian approximations, this model embraces the inherent nonlinearity of SLAM, benefiting significantly from the structural properties of matrix Lie groups such as $\mathbb{SE}(3)$ and $\mathbb{SO}(3)$.

Important to this approach is a continuous form filter designed for seamless operation on the stated Lie group, accommodating 6-DoF pose estimation which is critical for 3D space navigation tasks. Additionally, the filter effectively contends with unknown bias present in velocity measurements, a common challenge in practical SLAM applications.

Numerical Robustness and Empirical Evaluation

The simulation experiments illustrate the robustness of this novel nonlinear filter, particularly in discrete forms, underscoring its practical utility in demanding environments. These results are crucial, suggesting improved resilience against noise and systematic errors when estimating both pose and environmental features. Specifically, the quaternion representation of the nonlinear filter equips SLAM with enhanced capability in handling orientation dynamics more gracefully than traditional linear approximations.

Implications and Future Directions in AI Research

The implications of this research are significant both practically and theoretically. Practically, it proposes a reliable and computationally efficient alternative capable of scaling to complex SLAM operations needed in environments where traditional absolute positioning systems, like GPS, are ineffective. Theoretically, it opens new avenues in utilizing differential geometry and Lie group structures to build more accurate and robust localization and mapping algorithms.

Future developments might focus on integrating machine learning components with these geometric filters to further enhance their adaptability and performance in diverse operational contexts. Additionally, examining deployment in real-world scenarios, such as autonomous vehicular navigation or robotic exploration, could offer further validation and insights into broader applications.

In summary, this research advances the field of SLAM by providing a nonlinear observer that aligns closely with the true dynamics of 3D spatial navigation problems, potentially setting a new standard for localization strategies in autonomous systems.