Single Beacon based Localization with Constraints and Unknown Initial Poses

This paper studies a single beacon-based three-dimensional multirobot localization (MRL) problem. Unlike most of existing localization algorithms which use extended Kalman filter or maximum a posteriori, moving horizon estimation (MHE), and convex optimization are novelly designed to perform MRL with constraints and unknown initial poses. The main contribution of this paper is three-fold: 1) a constrained MHE-based localization algorithm, which can bound localization error, impose various constraints and compromise between computational complexity and estimator accuracy, is proposed to estimate robot poses; 2) constrained optimization is examined in the perspective of Fisher information matrix to analyze why and how multirobot information and constraints are able to reduce uncertainties; 3) a semidefinite programming-based initial pose estimation, which can efficiently converge to global optimum, is developed by using convex relaxation. Simulations and experiments are conducted to verify the effectiveness of the proposed methods.