On the basis of tight
coupling of the SINS and the USBL, a dual-
hydrophone differential model is provided, constant errors in USBL measurement are eliminated, and the influence of
slant range noise on navigation precision is relieved; a measurement equation of a dual-
hydrophone differential model is established, so that the precision and reliability of the integrated
navigation system are improved; a robust iterative
Kalman filtering algorithm based on approximate least-square
estimation is introduced, measurement dimensions are comprehensively evaluated, measurement
noise covariance is dynamically adjusted, and the
numerical stability and robustness of the
algorithm are improved. According to the method, whether the measured data at the current moment is an abnormal value or not is judged according to previous data, the robust weight is reasonably distributed to the measured data, the adverse effects of subsequent steps such as state
estimation are reduced through a method of reducing the weight of the abnormal value, and normalization operation is carried out on the measured residual error according to the adverse effects, so that the residual error distribution is more regular, and the accuracy is improved. And the updating result of the measurement
noise covariance is further optimized, so that the precision of the whole
Kalman filtering algorithm is improved.