Constrained submap algorithm for simultaneous localization and mapping

When solving the problem of simultaneous localization and mapping (SLAM), a standard extended Kalman filter (EKF) is subject to linearization errors and causes optimistic estimation. This paper proposes a submap algorithm, which builds a weighted least squares (WLS) constraint between two adjacent submaps according to the different estimations of the common features and the relationship between the vehicle poses in the corresponding submaps. By establishing the constraint equation after loop closing, re-linearization is implemented and each submap’s reference frame tends to its equilibrium position quickly. Experimental results demonstrate that the algorithm could get a globally consistent map and linearization errors are limited in local regions.