Cubature Kalman Filter based point set registration for SLAM

Point set registration is an important problem in Simultaneous Localization And Mapping (SLAM). In this paper, a novel registration algorithm based on Cubature Kalman Filter (CKF) is proposed. First, the state space model (SSM) regarding two point sets with Gaussian noise is formulated. Then CKF and simulated annealing based algorithm is utilized to optimize the process recursively. Since point set registration problem in SLAM application is mainly 3D, algorithm proposed in this paper focuses on 3D rigid point set registration problem. Compared to classical method, like Iterative Closest Point (ICP), CKF based method is more robust to noise and outliers. Besides that, this algorithm does not depend much on proper initialization which is very important to ICP. In order to explore the state space more efficiently, continuous simulated annealing is used on the process noise. Compared to Unscented Kalman Filter (UKF) based registration algorithm, this algorithm can improve precision of approximating nonlinearity in measurement update. So it will converge to the desired minimum more quickly, which is very important to SLAM considering real-time requirement. Experiments on SLAM dataset validate good performance of CKF based algorithm. The results also demonstrate the proposed method outperforms ICP and UKF based methods in the presence of noise, outliers and initial misalignment.