Novel FastSLAM algorithm based on square root unscented Kalman filter

Standard FastSLAM algorithm suffers from particle set degeneracy and accumulation errors caused by linearization of the nonlinear model.To overcome the above problems,this paper proposed a novel FastSlam algorithm based on square root unscented Kalman filter(SR-UKF).SR-UKF selected a group of representative sigma points to approximate the covariance,these sigma points were propageted through the non-linearforce model to reconstruct the new statistical characteristics.Using SR-UKF to replace EKF for posteriori estimation of particles could reduce the linearization error and slow down particle set degeneracy.SR-UKF ensured the non-negative definite of covariance matrix to guarantee the stability of SLAM algorithm.The simulation experiments demonstrate that the proposed algorithm is better than FastSLAM 2.0 both in accuracy and robustness.