Rao-blackwellised particle filter SLAM with consistent mapping for AUVs

The particle filter has emerged as a useful tool for problems requiring dynamic state estimation. Recent years, people have been improving the particle filter algorithm constantly. The consistency of the algorithm has extremely important impact on the accuracy of the whole algorithm. In this paper, we propose an improved Rao-Blackwellized particle filter (RBPF) employing the First-Estimates Jacobian(FEJ) in the landmarks estimation of the particle filter to improve the accuracy of landmarks estimation, whereby a consistent mapping will be obtained, which can deservedly improve the consistency of the entire particle filter. The consistency problem of the algorithm is analyzed utilizing the normalized estimation error square(NEES), and the validity of the improved algorithm is verified by the sea trial using our own AUV platform, C-ranger.