An Improved FastSLAM Algorithm Based on Revised Genetic Resampling and SR-UPF

FastSLAM is a popular framework which uses a Rao-Blackwellized particle filter to solve the simultaneous localization and mapping problem (SLAM). However, in this framework there are two important potential limitations, the particle depletion problem and the linear approximations of the nonlinear functions. To overcome these two drawbacks, this paper proposes a new FastSLAM algorithm based on revised genetic resampling and square root unscented particle filter (SR-UPF). Double roulette wheels as the selection operator, and fast Metropolis-Hastings (MH) as the mutation operator and traditional crossover are combined to form a new resampling method. Amending the particle degeneracy and keeping the particle diversity are both taken into considerations in this method. As SR-UPF propagates the sigma points through the true nonlinearity, it decreases the linearization errors. By directly transferring the square root of the state covariance matrix, SR-UPF has better numerical stability. Both simulation and experimental results demonstrate that the proposed algorithm can improve the diversity of particles, and perform well on estimation accuracy and consistency.