A robust cooperative localization algorithm based on covariance intersection method for multi-robot systems

Cooperative localization is an arising research problem for multi-robot system, especially for the scenarios that need to reduce the communication load of base stations. This article proposes a novel cooperative localization algorithm, which can achieve high accuracy localization by using the relative measurements among robots. To address uncertainty in the measuring robots' positions and avoid linearization errors in the extended Kalman filter during the measurement update phase, a particle-based approximation method is proposed. The covariance intersection method is then employed to fuse preliminary estimations from different robots, guaranteeing a minimum upper bound for the fused covariance. Moreover, in order to avoid the negative effect of abnormal measurements, this article adopts the Kullback-Leibler divergence to calculate the distances between different estimations and rejects to fuse the preliminary estimations far from the estimation obtained in the prediction stage. Two simulations are conducted to validate the proposed algorithm. Compared with the other three algorithms, the proposed algorithm can achieve higher localization accuracy and deal with the abnormal measurement.