Guaranteed 2D Pose Graph SLAM With Bounded Noises: An Efficient Interval Approach

This paper focuses on developing a performance guaranteed state estimation algorithm for 2D pose graph problems for mobile robots. Different from probabilistic methods, the measurement noises are only assumed to be bounded without any prior knowledge about their distributions. Based on the interval analysis, we first propose a vanilla sequential contractor that iteratively uses edge-wise noise bounds to contract pose intervals at the nodes, which can provide the guaranteed feasible domains that contain the ground-truth values. Then, to improve the efficiency in solving large-scale pose graphs, an efficient batch contractor is developed by improving the update order and exploiting a relaxation of the nonlinear measurement functions. The effectiveness and efficiency of our approaches are validated on simulated and real-world datasets. Note to Practitioners—Pose graph is one of the most popular formulations for the state estimations of mobile robots. There have been many probabilistic algorithms for pose graphs based on the Gaussian-like measurement noise assumption. However, the measurement noises in many practical situations may not follow Gaussian distributions but have hard bounds. Consequently, the existing pose graph algorithms are far away from achieving the expected high reliability in the practical safety-critical applications such as autonomous driving. To achieve guaranteed performance, an efficient interval based approach is proposed for the large-scale pose graph problems with hard bound measurement noises. It can provide the guaranteed hard error bounds for the robot poses, which has the potential in uncertainty quantification, reliability analysis and outlier detection of safety-critical systems.