A Strong Tracking Square Root Central Difference FastSLAM for Unmanned Intelligent Vehicle With Adaptive Partial Systematic Resampling

An improved fast simultaneous localization and mapping (FastSLAM) algorithm based on the strong tracking square root central difference Kalman filter (STSRCDKF) with adaptive partial systematic resampling is proposed in this paper to solve the large-scale simultaneous localization and mapping (SLAM) problem for unmanned intelligent vehicle. In the proposed algorithm, STSRCDKF is composed of a strong tracking filter and a square root central difference Kalman filter. STSRCDKF is used to design an adaptive adjusting proposal distribution of the particle filter and to estimate the Gaussian densities of the landmarks. Moreover, an adaptive partial systematic resampling operation is carried out to reduce the degree of particle degeneracy and maintain the diversity of particles. The performance of the proposed algorithm is compared with that of central difference FastSLAM and FastSLAM2.0; the simulation results based on the simulator and two benchmark data sets verify that the proposed algorithm has better adaptability and robustness to respond with time-varying measurement noise. In addition, it reduces computational cost and improves state estimation accuracy and consistency. Furthermore, the validity of the proposed algorithm is verified by the experimental result in campus test site of Beijing University of Technology.