Nonlinear moving horizon state estimation for multi-robot relative localization

This paper presents a novel approach for a multi-robot system's relative localization (RL), where one or more robots are located and tracked with respect to another robot frame of reference. With a known initial estimate of a robot being tracked, the extended Kalman filter (EKF) has been shown to perform adequately well to achieve the RL. However, with an arbitrary initial estimate, EKF performance may become unstable and/or require a high number of iterations to achieve an acceptable tracking error. In this paper, moving horizon estimation (MHE) has been adopted to achieve the RL objective. Although MHE has been highlighted in the literature to be computationally intractable, in this work, an efficient algorithm based on Real Time Iteration (RTI) scheme has been exported using an automatic C code generation toolkit. The exported code is adapted for the RL problem and requires computational capacity of the order ones of milliseconds. The MHE performance is compared against the EKF in numerical simulations. Under arbitrary estimator initialization, the results confirms that MHE over performs EKF in terms of the number of iterations required for convergence while satisfying the real-time requirements.