A Square Root Unscented FastSLAM With Improved Proposal Distribution and Resampling

An improved square root unscented fast simultaneous localization and mapping (FastSLAM) is proposed in this paper. The proposed method propagates and updates the square root of the state covariance directly in Cholesky decomposition form. Since the choice of the proposal distribution and that of the resampling method are the most critical issues to ensure the performance of the algorithm, its optimization is considered by improving the sampling and resampling steps. For this purpose, particle swarm optimization (PSO) is used to optimize the proposal distribution. PSO causes the particle set to tend to the high probability region of the posterior before the weights are updated; thereby, the impoverishment of particles can be overcome. Moreover, a new resampling algorithm is presented to improve the resampling step. The new resampling algorithm can conquer the defects of the resampling algorithm and solve the degeneracy and sample impoverishment problem simultaneously. Compared to unscented FastSLAM (UFastSLAM), the proposed algorithm can maintain the diversity of particles and consequently avoid inconsistency for longer time periods, and furthermore, it can improve the estimation accuracy compared to UFastSLAM. These advantages are verified by simulations and experimental tests for benchmark environments.