Square-Root Cubature FastSLAM Algorithm for Mobile Robot Simultaneous Localization and Mapping

摘要

In this paper, we derive a new SRCFastSLAM algorithm to SLAM problem, which is the square-root edition of our previously proposed Cubature FastSLAM. The main contribution lies that: 1) in SRCFastSLAM, the particles for SLAM implementation are assembled by means and covariance square-root factors (rather than covariances) of the robot state and the feature landmarks; 2) Due to the covariance square-root factors are directly propagated in our SLAM process, the time-expensive Cholesky decompositions on covariance matrixes are avoided, also the symmetry and positive (semi) definiteness of the covariance matrixes are preserved. The performance of the proposed algorithm is investigated and compared with FastSLAM2.0 and UFastSLAM using a serial simulation. Results show that the proposed SRCFastSLAM outperforms FastSLAM2.0 and UFastSLAM in precision and reduces the computational cost of the CFastSLAM obviously.