The Application of Square-Root Cubature Kalman Filter in SLAM for Underwater Robot

For simultaneous localization and mapping(SLAM) of underwater robots, the extended kalman filter algorithm has the problems of slow convergence rate, low accuracy and poor numerical stability, and square root cubature Kalman filter SLAM algorithm (SRCKF-SLAM) for mobile robots is designed according to cubature Kalman filter (CKF) principle. The cubature rule is utilized to calculate Gaussian weighted integral of the nonlinear function and robot pose particle's weight. The square root of the covariance is usedto replacethe system covariance matrix, which reduces the influence of truncation error on precision of SLAM. Simulation results show that the SRCKF algorithm has the advantages of high estimation accuracy, fast speed and stable convergence, which verify the effectiveness of the algorithm.