CFastSLAM: A new Jacobian free solution to SLAM problem

While FastSLAM algorithm is a popular solution to SLAM problem, it suffers from two major drawbacks: one is particle set degeneracy due to lack of observation information in proposal distribution; the other is errors accumulation caused by inaccuracy linearization of the robot motion model and the observation model. To overcome the problems, we propose a new Jacobian free CFastSLAM algorithm. The main contribution of this work lies in the utilization of Cubature Kalman Filter (CKF), which calculate Gaussian Weight Integral based on Cubature Rule, to design an optimal proposal distribution of the particle filter and to estimate the environment feature landmarks. On the basis of Rao-Blackwellized particle filter, proposed algorithm is comprised by two main parts: in the first part, a Cubature Particle Filter (CPF) is derived to localize the robot; in the second part, a set of CKFs is used to estimate the environment landmarks. The performance of the CFastSLAM is investigated and compared with that of FastSLAM2.0 and UFastSLAM in simulations and experiments. Results verify that the CFastSLAM improves SLAM performance.