Iterated SLSJF: A sparse local submap joining algorithm with improved consistency

Abstract

This paper presents a new local submap joining algorithm for building large-scale feature based maps. The algorithm is based on the recently developed Sparse Local Submap Joining Fil-ter (SLSJF) and uses multiple iterations to im-prove the estimate and hence is called Iterated SLSJF (I-SLSJF). The input to the I-SLSJF algorithm is a sequence of local submaps. The output of the algorithm is a global map con-taining the global positions of all the features as well as all the robot start/end poses of the local submaps. In the submap joining step of I-SLSJF, when-ever the change of state estimate computed by an Extended Information Filter (EIF) is larger than a predefined threshold, the information vector and the information matrix is recom-puted as a sum of all the local map contribu-tions. This improves the accuracy of the esti-mate as well as avoids the possibility that the Jacobian with respect to the same feature gets evaluated at different estimate values, which is one of the major causes of inconsistency for EIF/EKF algorithms. Although the computa-tional cost of I-SLSJF is higher than that of SLSJF, the algorithm can still be implemented efficiently due to the exactly sparseness of the information matrix. The new algorithm is com-pared with EKF SLAM and SLSJF using both computer simulation and experimental exam-ples. 1