Distributed Initialization for Visual-Inertial-Ranging Odometry with Position-Unknown UWB Network

摘要

In recent years, the visual-inertial-ranging (VIR) state estimator with a position-unknown UWB network has become popular. However, most existing VIR methods leverage centralized algorithms to initialize the UWB anchors, which are challenging to be applied to massive UWB networks. In this paper, we propose a distributed initialization method for consistent visual-inertial-ranging odometry with a position-unknown UWB network (DC-VIRO). For the position-unknown UWB anchors, we solve a Robot-aided Distributed Localization (RaDL) to initialize their positions. For robot state estimation, we fuse the ranging measurements of initialized anchors and visual-inertial measurements in a consistent filter. The RaDL is formulated as a consensus-based optimization problem and solved by the Distributed Alternating Direction Method of Multipliers (D-ADMM) algorithm. To identify the unobservable conditions, we propose a self-contained Fisher Information Matrix (FIM) based criterion which can be evaluated by each anchor directly with locally-preserved ranging measurements. We use Covariance Intersection (CI) to estimate the covariance of initialized anchors' positions for consistent data fusion. The proposed DC-VIRO is validated in both simulation and real-world experiments.