Enhancing Anchor Location Estimation Algorithm via Multi-Source Observations and Adaptive Optimization for UVIO

At present, the UWB-assisted VIO scheme only uses range measurements to estimate the anchor position. The accuracy of the anchor location estimation algorithm can be affected by factors such as the trajectory being a straight line or having a small curvature, as well as changes in multi-observation noise. To address these problems, we propose an adaptive UWB anchor location estimation algorithm leveraging Unmanned Ground Vehicle (UGV) multi-source observations. The key innovations include the following: (1) a novel anchor initialization method that incorporates both distance and angles, including azimuth and elevation measurements, to overcome the limitation of the approach that relies solely on range for straight or small-curvature trajectories; (2) an adaptive nonlinear optimization anchor location estimation algorithm that dynamically adjusts measurement weights and addresses the accuracy decreasing under time-varying noise characteristics in both distance and angle measurements caused by environmental disturbances. In this paper, the robustness and anchor position estimation accuracy of the proposed algorithm are validated through simulation and UGV real experiments.