Robust Range-Only Mapping via Sum-of-Squares Polynomials

Abstract

This work presents a new approach for mapping static beacons given only range measurements. An original formulation using sum-of-squares and linear matrix inequalities is derived to test if a measurement is inconsistent with a bounding box containing the beacon position. By performing this test for each range measurement, it is possible to recursively eliminate incompatible boxes and find the smallest consistent box. The box search is done with a breadth-first search algorithm that recursively prunes inconsistent boxes and splits the others to narrow the estimation. The validity of the method is asserted via simulations and compared to other standard mapping methods. Different levels and types of noise are added to evaluate the performances of the algorithm. It resulted that the approach accommodates very well classical zero-mean white Gaussian noises by adaptating the ratio of tolerated outliers for the consistency check, but fails to handle additive biases.