Abstract:
Standard FastSLAM algorithm suffers from the calculation of the Jacobian matrices and linearization error accumulation. To overcome these problems, a spherical simplex-radial cubature FastSLAM(SSRCFastSLAM) algorithm is proposed. The 3rd-degree spherical simplex-radial rule is utilized to calculate the nonlinear Gaussian weighted integral in order to improve SLAM accuracy. The proposed algorithm uses spherical simplex-radial cubature particle filter to estimate the path, and uses spherical simplex-radial cubature Kalman filter to maintain the landmarks. The performance of the proposed algorithm is compared with that of FastSLAM2.0, UFastSLAM and CFastSLAM through simulations and Victoria Park dataset. The results show that the proposed algorithm yields better localization and mapping ability than the other three algorithms with different particle numbers and noise levels, and the advantage is more significant when the number of particles is small or the environment disturbance is large, therefore the superiority of the proposed algorithm is verified.