Iterated Extended Kalman Filter Based on Particle Swarm Optimization and Its Application to Underwater Robot Navigation

In this paper, the high precision and robustness of underwater robot navigation based on strap-down inertial navigation system/ultra short base line (SINS/USBL) is investigated. The Extended Kalman Filter (EKF) is used to SINS/USBL system. An Iterated Extended Kalman Filter (IEKF) is proposed to enhance the robustness, which is based on Levenberg Marquardt (LM) and Laplace principles. IEKF state vector mean square error matrix is optimized based on LM algorithm. Meanwhile, measurement noise variance matrix is constructed based on Laplace principle. Considering the strong nonlinear problem of system noise and measurement noise, Particle Swarm Optimization (PSO) algorithm is introduced, and an Improved Particle Swarm Optimization (IPSO) algorithm is proposed. The mean-square error fitness function based on the position error of the navigation system is constructed. An improved inertia weight calculation method is proposed to further improve the adaptability of IPSO algorithm. The IPSO algorithm can adaptive estimate the system noise variance matrix and measurement noise variance matrix of IEKF. Finally, the SINS/USBL tightly integrated model is designed for proposed IPSO-IEKF algorithm, which based on slant and azimuth information. Simulation and experiment demonstrate the effectiveness of the proposed IPSO-IEKF algorithm. The experimental results show that the positioning accuracy of the proposed method is improved by 70% compared with EKF algorithm.