Stochastic Observer for SLAM on the Lie Group

Abstract: A robust nonlinear stochastic observer for simultaneous localization and mapping (SLAM) is proposed using the available uncertain measurements of angular velocity, translational velocity, and features. The proposed observer is posed on the Lie Group of $\mathbb{SLAM}_{n}\left(3\right)$ to mimic the true stochastic SLAM dynamics. The proposed approach considers the velocity measurements to be attached with an unknown bias and an unknown Gaussian noise. The proposed SLAM observer ensures that the closed loop error signals are semi-globally uniformly ultimately bounded. Simulation results demonstrates the efficiency and robustness of the proposed approach, revealing its ability to localize the unknown vehicle, as well as mapping the unknown environment given measurements obtained from low-cost units.