Optimal Initialization Strategies for Range-Only Trajectory Estimation

TL;DR

This paper introduces convex relaxation methods using semidefinite programming to improve initial trajectory estimation in range-only pose estimation, overcoming local minima issues and enhancing accuracy in static and dynamic scenarios.

Contribution

It presents novel SDP-based convex relaxations that provide accurate initial estimates for range-only trajectory estimation, addressing the challenge of local minima in nonconvex problems.

Findings

SDP relaxations yield accurate initial estimates in simulations.

Relaxations recover global minima under moderate noise.

Methods outperform traditional local solvers in accuracy.

Abstract

Range-only (RO) pose estimation involves determining a robot's pose over time by measuring the distance between multiple devices on the robot, known as tags, and devices installed in the environment, known as anchors. The nonconvex nature of the range measurement model results in a cost function with possible local minima. In the absence of a good initialization, commonly used iterative solvers can get stuck in these local minima resulting in poor trajectory estimation accuracy. In this work, we propose convex relaxations to the original nonconvex problem based on semidefinite programs (SDPs). Specifically, we formulate computationally tractable SDP relaxations to obtain accurate initial pose and trajectory estimates for RO trajectory estimation under static and dynamic (i.e., constant-velocity motion) conditions. Through simulation and real experiments, we demonstrate that our proposed initialization strategies estimate the initial state accurately compared to iterative local solvers. Additionally, the proposed relaxations recover global minima under moderate range measurement noise levels.