Geometric Nonlinear Observer Design for SLAM on a Matrix Lie Group

This paper considers the Simultaneous Localization and Mapping (SLAM) problem for a rigid body evolving in 3D space. We propose a nonlinear geometric observer, designed on the matrix Lie group SE
_1+n
(3), using group velocity and landmark measurements. The proposed observer is also extended to handle unknown biases in the linear and angular velocity. Simulation results are presented to illustrate the effectiveness of the proposed observer.