3-D Inter-Robot Relative Localization via Semidefinite Optimization

In this letter, the 3-D inter-robot relative localization problem is addressed using noise-corrupted odometric and distance measurements. Unlike the existing solutions, we are devoted to providing a relative localization method that has an “overall best performance,” which means that the tradeoffs between the estimation accuracy (EA), the number of measurements (NoMs), and the computation efficiency (CE) are considered. We demonstrate that an existing formulation of the 3-D relative localization problem, the square distances weighted least square (SD-WLS), can be equivalently reformulated as a non-convex quadratic constrained quadratic programming (QCQP) problem. Further, to handle the non-convex nature of the QCQP problem, we adopt the semidefinite programming (SDP) relaxation approach, which drops the rank constraint and recovers the solution of the QCQP via an eigenvalue decomposition strategy. Finally, a refinement step is introduced to solve the problem that the quadratic constraints might not be satisfied due to the SDP relaxation. The simulation and experiment results show that, compared to existing methods, our method has the best overall performance when the three factors, i.e., EA, NoMs, and CE, are important for a relative localization application.