Localization of Probabilistic Robot Formations in SLAM

This paper presents an EKF-based approach to the problem of robot formation pose tracking in SLAM when a previously built feature-based stochastic map of a navigation area is available. We show how a direct implementation of the EKF algorithm leads to inconsistency in the estimated localization. We justify the origin of the anomalous behaviour of the filter in the time-correlated nature of the measurement noise sequence. A novel solution based on the measurement differencing technique is proposed to drive the solution of the EKF towards consistency. Both simulation and real experiments with a 3-robot triangular-shaped formation are reported.