A Two-Stage Kalman Filter for Integrated Navigation System of Underwater Vehicle

The unmanned autonomous underwater vehicle (AUV) system cannot use GPS for accurate positioning when operating underwater, and the pure inertial guidance system has a large error in the dynamic process. In order to solve the problem, a combined navigation algorithm based on two-stage Kalman filter is proposed in this paper. The difference between the output speed of the micro-inertial navigation and DVL is taken as the first measurement of the filter, and then the difference between the calculated magnetic heading and the heading obtained by a feedback correction is used as the second measurement, so as to obtain high-precision navigation parameters and improve the positioning accuracy of the system. According to the experimental results, it can be seen that the algorithm in this paper realizes the high-precision estimation of heading and attitude, and the heading error is kept within the expectation, which greatly improves the positioning accuracy of the system.