A Resilient RO-SLAM Algorithm with Bearing Reconstruction of Detected Landmarks

In this paper we consider a Range Only (RO) SLAM problem for a unicycle like robot. The solution approach is based on a bearing reconstruction for each responding landmark obtained through a two dimensional EKF, which fuses odometry readings of the robot with the available range measurements to produce a range and bearing estimate of the considered landmark. These estimates, regarded as measurements with a proper covariance matrix, are used in an EKF SLAM algorithm. A resilient module is added to the algorithm to cope with the initial uncertainty characterizing the bearing estimates, but also to resist the effects of outliers and to detect possible abnormal situations. Simulation results show the effectiveness of the proposed approach which appears robust to several kinds of disturbances and provides estimation errors comparable to the case the bearing is directly available from the sensor and the range measurements have the same variance.