Unscented FastSLAM: A Robust and Efficient Solution to the SLAM Problem

The Rao-Blackwellized particle filter (RBPF) and FastSLAM have two important limitations, which are the derivation of the Jacobian matrices and the linear approximations of nonlinear functions. These can make the filter inconsistent. Another challenge is to reduce the number of particles while maintaining the estimation accuracy. This paper provides a robust new algorithm based on the scaled unscented transformation called unscented FastSLAM (UFastSLAM). It overcomes the important drawbacks of the previous frameworks by directly using nonlinear relations. This approach improves the filter consistency and state estimation accuracy, and requires smaller number of particles than the FastSLAM approach. Simulation results in large-scale environments and experimental results with a benchmark dataset are presented, demonstrating the superiority of the UFastSLAM algorithm.