State-transition and observability constrained EKF for multi-robot cooperative localization

This paper introduces a new extended Kalman filter (EKF) for multi-robot cooperative localization (CL), termed statetransition and observability constrained (STOC)-EKF, aiming to improve estimation consistency and accuracy. In particular, it has been shown that the standard EKF linearized CL system has observability properties different from those of the underlying nonlinear system, which causes inconsistent estimates. We further analytically prove in this paper that the propagation Jacobians of the standard EKF CL violate semi-group properties, and thus are not valid state-transition matrices. This implies that the linearized dynamical system does not well approximate the dynamics of the underlying nonlinear system and thus degrades estimation performance. To address these issues, the proposed STOC-EKF (i) computes the propagation Jacobian always using prior state estimates as linearization points, and (ii) projects the most accurate measurement Jacobian at each time step (i.e., computed using the latest, and thus best, state estimates as in the standard EKF) onto the observable subspace. Extensive Monte-Carlo simulations validate the proposed algorithm.