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. Note to Practitioners—The motivation of this paper is the combined positioning and anti-interference issues of the SINS/USBL integrated navigation system for underwater robots. EKF is a commonly used underwater integrated navigation algorithm. However, it often fails to address nonlinear constrained issues present in practical systems during application. Therefore, an Iterated Extended Kalman Filter (IEKF) is proposed to enhance the robustness, which is based on LM and Laplace principles. On the other hand, PSO algorithm is introduced in IEKF, and an improved PSO algorithm based on IEKF is proposed. Simultaneously, the proposed IPSO-IEKF algorithm enhances the system’s robustness against disturbances. The proposed method effectively addresses the nonlinear constrained issues in underwater navigation systems, thereby enhancing system stability. Simulation and experimentation demonstrate the feasibility of the method studied in this paper for practical engineering applications.