An Improved FastSLAM Algorithm for Autonomous Vehicle Based on the Strong Tracking Square Root Central Difference Kalman Filter

Fast simultaneous localization and mapping (FastSLAM), a popular algorithm based on the Rao-Blackwellized Particle Filter, has been used to solve the large-scale simultaneous localization and mapping (SLAM) problem for autonomous vehicle, but it suffers from two serious shortcomings: one is the calculation of Jacobian matrices and the linear approximations of the nonlinear vehicle kinematics model and the nonlinear environment measurement model; the other is particle set degeneracy due to inaccurate proposal distribution of particle filter. Hence an improved FastSLAM algorithm based on the strong tracking square root central difference Kalman filter (STSRCDKF) is proposed in this paper to overcome these problems. In the proposed algorithm, STSRCDKF is based on the combination of a strong tracking filter (STF) and a square root central difference Kalman filter (SRCDKF), STSRCDKF is used to design an adaptive adjustment proposal distribution of the particle filter and to estimate the Gaussian densities of the feature landmarks. The performance of the proposed algorithm is compared with that of UFastSLAM and FastSLAM2.0 in simulations and experimental tests, the results verify that the proposed algorithm has better adaptability and robustness. Furthermore, it reduces computational cost and improves state estimation accuracy and consistency.