Robust Filter Algorithm for Underwater Navigation System Aided by Chi-square Information

Aiming at the influence of unknown and outnumber of measurement noise on the positioning accuracy of integrated navigation system in underwater complex environment. This paper presents a robust filter algorithm of SINS/DVL/USBL integrated navigation aided by chi-square information. Firstly, an optimal state estimation method based on Bayesian estimation is presented. On this basis, the cost function is introduced, and the robust Huber M estimation Kalman filter algorithm is given through the cost function calculation. Secondly, to solve the problem of continuous outlier interference, an improved Huber M filter algorithm based on the new chi-square detection method is proposed. The above method can suppress the influence of continuous non-Gaussian strong noise and improve the filtering accuracy and stability. Finally, the validity of the method is verified by the Yangtze River experiment.