Consistency of the EKF-SLAM algorithm

This paper presents an analysis of the extended Kalman filter formulation of simultaneous localisation and mapping (EKF-SLAM). We show that the algorithm produces very optimistic estimates once the "true" uncertainty in vehicle heading exceeds a limit. This failure is subtle and cannot, in general, be detected without ground-truth, although a very inconsistent filter may exhibit observable symptoms, such as disproportionately large jumps in the vehicle pose update. Conventional solutions-adding stabilising noise, using an iterated EKF or unscented filter, etc-do not improve the situation. However, if "small" heading uncertainty is maintained, EKF-SLAM exhibits consistent behaviour over an extended time period. Although the uncertainty estimate slowly becomes optimistic, inconsistency can be mitigated indefinitely by applying tactics such as batch updates or stabilising noise. The manageable degradation of small heading variance SLAM indicates the efficacy of submap methods for large-scale maps.