LAUSR

Aiming at the problem of outliers in Doppler velocity log (DVL) beam measurements, for strapdown inertial navigation system (SINS)/Doppler velocity log (DVL) tightly coupled system, an improved robust Kalman filter (KF) algorithm is proposed, which can handle the measurement noise variance of each DVL beam individually. First, the multidimensional measurement equation is decomposed into several 1-D measurement equations, i.e., different DVL beam measurements are assumed to be obtained by different sensors. Second, the statistical similarity measure (SSM) is introduced to quantify the similarity between two random vectors. And a new cost function based on SSM is constructed for the dynamic system containing one state equation and multiple measurement equations. To optimize this cost function, the iterative procedures to update the posterior state is derived based on the Gauss-Newton iteration algorithm. Both the simulation and lake trial illustrate the superiority and effectiveness of the proposed algorithm. It is shown that in the case of partial DVL beams with outliers, the proposed algorithm can avoid the loss of normal measurement information and thus improve estimation accuracy.