— This paper addresses the consistency issue of the Extended Kalman Filter approach to the simultaneous localization and mapping (EKF-SLAM) problem. Linearization of the inherent nonlinearities of both the motion and the sensor models frequently drives the solution of the EKF-SLAM out of consistency specially in those situations where location uncertainty surpasses a certain threshold. This paper proposes a robocentric local map sequencing algorithm which: (a) bounds location uncertainty within each local map, (b) reduces the computational cost up to constant time in the majority of updates and (c) improves linearization accuracy by updating the map with sensor uncertainty level constraints. Simulation and large-scale outdoor experiments validate the proposed approach.
Ruben Martinez-Cantin, José A. Castellanos