Resilient simultaneous localization and mapping fusing Ultra Wide Band range measurements and visual odometry

In this paper we consider a Simultaneous Localization and Mapping (SLAM) problem for a moving agent using Visual Odometry (VO) while measuring the range from a set of Ultra Wide Band (UWB) antennas, deployed in unknown position in the environment. The solution approach is based on a switching observer which, under standard working conditions, for each observed UWB, uses a two dimensional Extended Kalman Filter (EKF) providing an estimate of the range and bearing of the observed UWB with respect to the agent. This information is then used in a Robust EKF algorithm which solves the SLAM problem with performances that, even before closing the loop, are comparable to the ones that a VO algorithm (namely ORB-SLAM2) would obtain only after closing the loop. Moreover, a resilient module is added to the algorithm to evaluate the reliability of the position estimate of each observed UWB. When the Visual Odometry is not available, the switching observer uses an auxiliary EKF to provide an estimate of the agent position. This makes the proposed approach robust with respect to several kinds of unmodeled disturbances, like multipath effects, and automatically adapts to sensor failures with resilience (e.g. when Visual Odometry or UWB measurements are not available).