Robust Iterated Sigma Point FastSLAM Algorithm for Mobile Robot Simultaneous Localization and Mapping

Simultaneous localization and mapping (SLAM) is a key technology for mobile robots operating under unknown environment. While FastSLAM algorithm is a popular solution to the SLAM problem, it suffers from two major drawbacks: one is particle set degeneracy due to lack of observation information in proposal distribution design of the particle filter; the other is errors accumulation caused by linearization of the nonlinear robot motion model and the nonlinear environment observation model. For the purpose of overcoming the above problems, a new iterated sigma point FastSLAM (ISP-FastSLAM) algorithm is proposed. The main contribution of the algorithm lies in the utilization of iterated sigma point Kalman filter (ISPKF), which minimizes statistical linearization error through Gaussian-Newton iteration, to design an optimal proposal distribution of the particle filter and to estimate the environment landmarks. On the basis of Rao-Blackwellized particle filter, the proposed ISP-FastSLAM algorithm is comprised by two main parts: in the first part, an iterated sigma point particle filter (ISPPF) to localize the robot is proposed, in which the proposal distribution is accurately estimated by the ISPKF; in the second part, a set of ISPKFs is used to estimate the environment landmarks. The simulation test of the proposed ISP-FastSLAM algorithm compared with FastSLAM2.0 algorithm and Unscented FastSLAM algorithm is carried out, and the performances of the three algorithms are compared. The simulation and comparing results show that the proposed ISP-FastSLAM outperforms other two algorithms both in accuracy and in robustness. The proposed algorithm provides reference for the optimization research of FastSLAM algorithm.