Improvement of Simultaneous Localization and Mapping Based on Extended Kalman Filter using Iterative Closest Point Algorithm

The Extended Kalman Filter (EKF) is one of the most common and widely used nonlinear estimators employed for Simultaneous Localization and Mapping (SLAM) tasks. In this paper, by employing the Iterative Closest Point (ICP) algorithm in the prediction step based on the updated information at the previous time, the accuracy of the EKF-based SLAM has been significantly improved. In the proposed method, we improve the accuracy of the robot's position estimation by matching two consecutive observations of the robot from its surroundings and then by using the position coordinates of reobserved landmarks between these two consecutive observations. Simulation results show that incorporating the ICP algorithm into the EKF-based SLAM significantly improves the accuracy of robot and landmarks position estimation. This method also exhibits better convergence and stability compared to the EKF-based SLAM.