Estimating odometry scale and UWB anchor location based on semidefinite programming optimization

Abstract

In this letter, we study the problem of estimating the unknown metric scale of an odometry system and the 3D location of an Ultra-wideband (UWB) anchor in the environment. Firstly, we present a theoretical analysis of the problem which includes the derivation of Fisher Information Matrix (FIM) and its determinant. Secondly, based on the FIM we provide an evaluation and geometric interpretation of singular configurations. Thirdly, we present an estimation-Trajectory optimization framework, which consists of a semidefinite programming (SDP) relaxation approach that solves the problem more effectively by exploiting the relationship between the parameters, and an FIM-based trajectory optimization approach that aims to minimize the uncertainty volume while remains easily adaptable to various scenarios. Simulation results show that our estimation method is more accurate and robust than previous approaches, while our trajectory optimization method can improve the estimator's performance even under challenging constraints.