Gradient-Based Observer for Simultaneous Localization and Mapping

An observer for simultaneous localization and mapping is considered in this paper. The proposed observer is based on recent theoretical foundations in a gradient-based observer design on Lie groups. As such, the form of the observer is similar to recently developed nonlinear attitude and pose observers. Translational and angular velocity measurements, as well as relative position measurements of nearby landmarks, are used directly within the observer structure. The case of biased translational and angular velocity measurements is also considered. Stability results are presented that demonstrate an asymptotic convergence of the pose and map estimates. The proposed algorithm is implemented successfully in experiment.