Real-time Optimized ESEKF-SLAM Algorithm for ArUco Landmark Array

Taking into account the problems of low precision map in the landmark arrays based on manual calibration. An automatic mapping algorithm for large-scale landmark array is proposed in this paper, which is mainly based on the multi-source information fusion algorithm. Through introducing the error state modeling theory and the indirect Kalman filter theory in the EKF-SLAM structure, the singularity of the covariance matrix is avoided. Meanwhile, an joint optimization framework based on the key frame method and the nonlinear optimization theory is proposed, which effectively reduce the cumulative error in map. Meanwhile, the efficiency of the algorithm is ensured by reducing the dimension of the filter after each optimization. By comparing with the existing mapping algorithm based on pure image recognition. The experimental results provide that the complete map with centimeter-level global precision can be built based on the proposed algorithm.