Graph Optimization Approach to Localization with Range Measurements

A range-based localization system is proposed based on graph optimization with ultra-wideband (UWB) measurements. There are still multiple issues for existing algorithms to be applied directly in real-time low power systems, especially for micro drones. For instance, the accuracy of triangularization is relatively low since it neglects the time differences of measured distances for position calculation. The filtering methods such as extended Kalman filter (EKF) are sensitive to measurement outliers and need accurate kinetic model. The machine learning methods are computational expensive and performance is unstable in different environments. To overcome the above problems, we propose to construct a joint trajectory smoothness and range constraint in a pose graph, which estimates robot poses over a sliding trajectory window at a single time instant. This removes the dependence on kinetic model and makes the system flexible and portable. Our algorithm is robust to outliers and even short-term measurement loss yet still with very low complexity. Extensive experiments under a variety of scenarios verify its stability and demonstrate a much higher accuracy than existing methods, especially in the attitude direction, which is challenging for existing methods due to an asymmetrical anchor distribution.