A method of underwater integrated navigation based on isr-ukf

Abstract

The invention provides a kind of underwater integrated navigation method based on ISR-UKF, comprising: respectively establishing the state equation and the measurement equation of the SINS / DVL subsystem in the underwater diving stage and the SINS / GPS subsystem in the water surface position correction stage; The non-linear filter equation of the SINS / DVL subsystem in the submerged stage, through the iterative square root unscented Kalman filter algorithm, carries out the navigation in the underwater submerged stage; after a long time underwater submerged, it can reach near the water surface by controlling the submerged depth of the AUV Stay for a short time, obtain GPS position and speed information assistance, establish the nonlinear filter equation of the SINS / GPS subsystem in the water surface position correction stage, and realize the correction of the deviation of the AUV underwater submerged stage through iterative square root unscented Kalman filter algorithm. So as to achieve the purpose of AUV sailing along the designated route. The invention can improve the filter calculation efficiency of the SINS / DVL / GPS underwater integrated navigation system, and is easier to program and realize under the premise of ensuring real-time performance and stability.

Description

technical field [0001] The invention belongs to the field of inertial navigation, and relates to an underwater integrated navigation method based on an Iterated Square-Root Unscented Kalman Filter (ISR-UKF). Background technique [0002] Real-time high-precision navigation and positioning services are the key to the effective operation and safe recovery of AUVs (ie autonomous underwater vehicles). However, the marine environment is very different from the terrestrial environment. It is dynamic, unknown and complex. The special underwater environment increases the difficulty of underwater navigation and positioning. Therefore, some effective means are used to improve the underwater navigation and positioning. Accuracy has always been the focus of attention. In addition, the output data of a single sensor has a certain degree of uncertainty, which is embodied in the incompleteness, inaccuracy or unreliability of the data. For example, inertial navigation is a kind of autonom...

AI Technical Summary

Problems solved by technology

EKF (extended Kalman filter) based on Kalman filter is easy to implement and has been widely used in various engineering fields; but EKF also has its own insurmountable limitations, such as many systems in practical applications do not exist Jacobian matrix, at which point linearization of the nonlinear function does not work

However, UKF (that is, unscented Kalman filter) uses unscented transformation to replace the local linearization in EKF. It does not need to calculate the Jacobian matrix, and does not need to derivate the Jacobian matrix. Therefore, it is not required that the state function and the measurement function must be It is continuously differentiable and does not require the system to be approximately linear; however, in the UKF filtering process, each sampling point needs to be transformed nonlinearly, which requires a large amount of calculation, and there are often rounding errors in numerical calculations that may destroy the system The non-negative definiteness and symmetry of the estimated error covariance matrix affect the convergence speed and stability of the filtering algorithm

Images