Aiming at the complex underwater environment which leads to large measurement errors and outliers in ultrashort baseline, a robust iterative Kalman filtering algorithm based on the differential model of dual hydrophones is proposed in this paper. Firstly, to achieve accurate navigation under large initial misalignment angles, the algorithm builds an inertial navigation error equation based on the right invariant error definition of Lie groups. Then a dual hydrophone differential model is proposed based on SINS/USBL tightly coupled. It successfully improves the accuracy of integrated navigation and significantly mitigating the issue of SINS/USBL positioning accuracy degradation caused by USBL slant distance noise that grows with distance. Finally, a robust iterative Kalman filter based on the dual hydrophone differential model is proposed to counter the threat to the vehicle’s safe operation and the accuracy loss caused by outliers. The algorithm’s superiority and effectiveness are verified through simulation and sea trial.
Read full abstract