On the consistency of multi-robot cooperative localization

Abstract

In this paper, we investigate the consistency of extended Kalman filter (EKF)-based cooperative localization (CL) from the perspective of observability. To the best of our knowledge, this is the first work that analytically shows that the error-state system model employed in the standard EKF-based CL always has an observable subspace of higher dimension than that of the actual nonlinear CL system. This results in unjustified reduction of the EKF covariance estimates in directions of the state space where no information is available, and thus leads to inconsistency. To address this problem, we adopt an observabilitybased methodology for designing consistent estimators and propose a novel Observability-Constrained (OC)-EKF. In contrast to the standard EKF-CL, the linearization points of the OC-EKF are selected so as to ensure that the dimension of the observable subspace remains the same as that of the original (nonlinear) system. The proposed OC-EKF has been tested in simulation and experimentally, and has been shown to significantly outperform the standard EKF in terms of both accuracy and consistency.