Tightly-Coupled Single-Anchor Ultra-wideband-Aided Monocular Visual Odometry System

Abstract

In this work, we propose a tightly-coupled odometry framework, which combines monocular visual feature observations with distance measurements provided by a single ultra-wideband (UWB) anchor with an initial guess for its location. Firstly, the scale factor and the anchor position in the vision frame will be simultaneously estimated using a variant of Levenberg-Marquardt non-linear least squares optimization scheme. Once the scale factor is obtained, the map of visual features is updated with the new scale. Subsequent ranging errors in a sliding window are continuously monitored and the estimation procedure will be reinitialized to refine the estimates. Lastly, range measurements and anchor position estimates are fused when needed into a pose-graph optimization scheme to minimize both the landmark reprojection errors and ranging errors, thus reducing the visual drift and improving the system robustness. The proposed method is implemented in Robot Operating System (ROS) and can function in real-time. The performance is validated on both public datasets and real-life experiments and compared with state-of-the-art methods.