Robust simultaneous localization and mapping via information matrix estimation

One of the major challenges for current SLAM systems is the impact of outliers and incorrectly modeled measurement noise on the final mapping solution. Outliers, such as incorrect loop closure detections, can cause standard least-squares based SLAM algorithms to fail catastrophically. This paper presents and evaluates a robust SLAM algorithm that addresses this issue by directly estimating the measurement information matrices along with the poses during the optimization. By inferring the information matrices, the algorithm can compensate for situations where the measurement covariances have been set incorrectly. Additionally, the information matrix estimates provide useful metrics for detecting incorrect loop closures and excising them from the SLAM solution. Because the algorithm consists of a closed-form update for the information matrices followed by a standard nonlinear least-squares update for the poses, the runtime is comparable to state-of-the-art non-robust SLAM algorithms while providing significantly more accurate results. Monte Carlo simulations also demonstrate that the proposed algorithm can match the runtime and error performance of alternative state-of-the-art robust SLAM algorithms. Finally, a sensitivity study shows that the proposed algorithm has a wide basin of convergence with respect to its tuning parameter.