The application of square-root cubature Kalman filter and probability hypothesis density in simultaneous localization and mapping for mobile robots

Abstract

A simultaneous localization and mapping(SLAM) algorithm based on square-root cubature Kalman filter and probability hypothesis density(SRCKF-PHD) is proposed, which is applied to situations of high clutter or ambiguous data association. The main contributions are: 1) to improve the performance of robot pose estimation, the cubature rule is utilized to calculate Gaussian weighted integral of the nonlinear function and robot pose particle's weight; 2) in the process of GM-PHD update, SRCKF is utilized for calculating measurement likelihood and Gaussian component's weight, which guarantees the symmetry and positive semi-definiteness of the covariance matrix and improves the numerical stability and accuracy. The proposed algorithm is compared with the RB-PHD-SLAM algorithm in simulation and Car Park data set.The results show that the proposed algorithm outperforms RB-PHD-SLAM algorithm.