Single beacon based multi-robot cooperative localization using Moving Horizon Estimation

This paper studies three-dimensional multi-robot Cooperative Localization (CL) problem. Most of existing CL strategies adopt Extended Kalman Filter (EKF) or Maximum a Posteriori (MAP). In this paper, a novel approach based on Moving Horizon Estimation (MHE) is proposed. The main contribution of this paper is twofold: 1) MHE is integrated with EKF for three-dimensional CL using single mobile beacon, which can bound localization error, impose various constraints on states and noises, and make use of previous range measurements for current estimation. 2) A sufficient condition on observability of multi-robot CL is derived by using Fisher Information Matrix. Simulation is conducted to verify that the proposed MHE based CL algorithm outperforms EKF based method in terms of localization accuracy, and two scenarios where our algorithm is superior to EKF are discussed.