Navigation and positioning system for underwater glider and up floating error correction method

Abstract

A navigation and positioning system for an underwater glider includes a micro-electro-mechanical system inertial measurement unit, a global positioning system receiving module, a triaxial magnetometer, a Doppler velocimeter, and an integrated navigation hardware processing system. If a navigation and positioning error is too large, the underwater glider stops working in real time and switches from the underwater working state to the up floating error correction state; and when velocity and location errors of a GPS/INS integrated navigation system are smaller than specified thresholds, the underwater glider switches from the up floating error correction state to the underwater working state and continues to work. An H Kalman filter algorithm based on an adaptive multiple fading factor is established to ensure robustness and adaptability of navigation and positioning of a glider.

Claims

1. A navigation and positioning system for an underwater glider, comprising a micro-electro-mechanical system inertial measurement unit (MEMS-IMU), a global positioning system (GPS) receiving module, a triaxial magnetometer, a Doppler velocimeter (DVL), and an integrated navigation hardware processing system, wherein the MEMS-IMU integrates a triaxial accelerometer and a triaxial gyroscope, and triaxial acceleration and angular rate information are output for obtaining navigation information comprising an attitude, a velocity, and a location of the underwater glider by using an inertial navigation algorithm; the triaxial magnetometer is configured to correct a heading information error of the underwater glider, and the DVL is configured to determine a motion state of the underwater glider; the GPS receiving module is combined with the MEMS-IMU to form an integrated navigation system for a correction of GPS/inertial navigation system (INS) location and velocity errors in an up floating error correction operation, and the GPS receiving module is configured to correct velocity and location errors of the underwater glider by using an integrated navigation filter algorithm; and the integrated navigation hardware processing system is configured to receive, process, and perform a clock synchronization on signals from the triaxial magnetometer, the GPS receiving module, the MEMS-IMU, and the DVL, and perform an algorithm calculation of integrated navigation multi-sensor information; and wherein further, the navigation and positioning system has an underwater working state and an up floating error correction state; if a navigation and positioning error is large, the underwater glider stops working in real time and switches from the underwater working state to the up floating error correction state; and when velocity and location errors of a GPS/INS integrated navigation system are smaller than specified thresholds, the underwater glider switches from the up floating error correction state to the underwater working state and continues to works, wherein for up floating error correction, signals output by the GPS receiving module and the MEMS-IMU are used for performing a navigation and a positioning of the underwater glider by using the integrated navigation system for the correction of the GPS/INS location and the velocity errors, wherein an H Kalman filter algorithm based on an adaptive multiple fading factor is used as the integrated navigation filter algorithm; and a state prediction covariance matrix of the H Kalman filter algorithm is P.sub.k|k-1=S.sub.k.sub.k|k-1P.sub.k-1.sub.k|51.sup.TS.sub.k.sup.T+Q.sub.k-1, an H filter gain matrix is K.sub.k=P.sub.k|k-1H.sub.k.sup.T(H.sub.kP.sub.k|k-1H.sub.k.sup.T+I).sup.1, and an H filter state optimal covariance matrix is $P_k=P_{k|k-1}-{}_{k|k-1}{P}_{k|k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k|k-1}{}_{k|k-1}^T,$ wherein k and k-1 represent a current moment and a previous moment respectively, .sub.k|k-1 is a state-transition matrix, H.sup.k is an observation matrix, Q.sub.k-1 is a system noise covariance, S.sub.k is an adaptive multiple fading factor matrix, L.sub.k is an estimate of a linear combination of system state variables, .sub.i is a unit matrix, $R_{e,k}=\left[\begin{array}{cc}I& 0\\ 0& -{}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k|k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right],$ and is an adaptive threshold, and wherein the adaptive multiple fading factor matrix S.sub.k=diag (s.sub.1, s.sub.2, s.sub.3 . . . , s.sub.n) is calculated by using the following formula: $s_i=\{\begin{array}{cc}\max \left(1,\sqrt{\frac{{\left[{}_i\left(k\right)\right]}^2}{{}_i^2{j}_{\mathrm{ii}}\left(k\right){}_i}-\frac{b_{\mathrm{ii}}\left(k\right)}{j_{\mathrm{ii}}\left(k\right)}}\right),& \mathrm{observable}\mathrm{variables}\\ 1,& \mathrm{unobservable}\mathrm{variables}\end{array},$ wherein .sub.i is an i.sup.th observation element of the observation matrix H.sub.k j.sub.ii (k) is an i.sup.th diagonal element of a matrix J.sub.k=.sub.k|k-1P.sub.k-1.sub.k|k-1.sup.T .sub.i is a threshold of a Chi-square test, v.sub.i (k) is an i.sup.th diagonal element of an innovation matrix V.sub.k and b.sub.ii (k) is an i.sup.th diagonal element of a matrix B.sub.k=H.sub.kQ.sub.k-1H.sub.k.sup.T+R.sub.k wherein the adaptive threshold is =.Math..sub.a, wherein $=\frac{V_k^T{V}_k+\mathrm{Trace}\left(H_k{P}_{k|k-1}{H}_k^T+R_{k-1}\right)}{V_k^T{V}_k},$ .sub.a=(L.sub.k.sup.TL.sub.k(.sub.k.sup.1+H.sub.k.sup.TH.sub.k).sup.1) Trace( ) represents a matrix trace operation, and ( ) represents a spectral radius of a matrix.

2. The navigation and positioning system according to claim 1, wherein for an underwater operation, signals output by the MEMS-IMU and the triaxial magnetometer are used for performing a navigation and a positioning of the underwater glider by using a quaternion algorithm based on complementary filtering.

3. An up floating error correction method for a navigation and positioning system for an underwater glider, comprising the following steps: (1) determining whether an underwater glider needs to perform an up floating error correction operation or not, and when a heading variation is large, a velocity variation is large, or manually determining that a navigation and positioning error of the underwater glider is large, floating, by the underwater glider, up to a water surface, to perform the up floating error correction operation; (2) receiving, after the underwater glider floats up to the water surface, a longitude and latitude signal, an altitude signal, and a triaxial velocity signal from a global positioning system (GPS) receiving module, and a triaxial acceleration signal and a triaxial angular velocity signal from an inertial measurement unit (IMU), and applying an H Kalman filter algorithm based on an adaptive multiple fading factor to an integrated navigation system for a correction of GPS(global positioning system)/inertial navigation system (INS) location and velocity errors, to perform a data fusion; and (3) reducing, through step (2), velocity and location errors of the integrated navigation system gradually to approach zero, and if the velocity and location errors are smaller than specified thresholds, indicating that the up floating error correction is completed, switching, by the underwater glider, to an underwater working state, wherein a state prediction covariance matrix of the H Kalman filter algorithm based on the adaptive multiple fading factor is P.sub.k|k-1=S.sub.k.sub.k|k-1P.sub.k-1.sub.k|51.sup.TS.sub.k.sup.T+Q.sub.k-1, an H filter gain matrix is K.sub.k=P.sub.k|k-1H.sub.k.sup.T(H.sub.kP.sub.k|k-1H.sub.k.sup.T+I).sup.1 and an H filter state optimal covariance matrix is $P_k=P_{k|k-1}-{}_{k|k-1}{P}_{k|k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k|k-1}{}_{k|k-1}^T,$ wherein k and k-1 represent a current moment and a previous moment respectively, .sub.k|k-1 is a state-transition matrix, H.sub.k is an observation matrix, Q.sub.k-1 is a system noise covariance, S.sub.k is an adaptive multiple fading factor matrix, L.sub.k is an estimate of a linear combination of system state variables, I is a unit matrix, $R_{e,k}=\left[\begin{array}{cc}I& 0\\ 0& -{}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k|k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right],$ and is an adaptive threshold, wherein the adaptive multiple fading factor matrix S.sub.k=diag (s.sub.1, s.sub.2, s.sub.3 . . . , s.sub.n) is calculated by using the following formula: $s_i=\{\begin{array}{cc}\max \left(1,\sqrt{\frac{{\left[{}_i\left(k\right)\right]}^2}{{}_i^2{j}_{\mathrm{ii}}\left(k\right){}_i}-\frac{b_{\mathrm{ii}}\left(k\right)}{j_{\mathrm{ii}}\left(k\right)}}\right),& \mathrm{observable}\mathrm{variables}\\ 1,& \mathrm{unobservable}\mathrm{variables}\end{array},$ wherein .sub.i is an i.sup.th observation element of the observation matrix H.sub.k-j.sub.ii (k) is an i.sup.th diagonal element of a matrix is a J.sub.k=.sub.k|k-1P.sub.k-1.sub.k|k-1.sup.T.sub.i threshold of a Chi-square test, u.sub.i (k) is an i.sup.th diagonal element of an innovation matrix V.sub.k, and b.sub.ii (k) is an i.sup.th diagonal element of a matrix B.sub.k=H.sub.kQ.sub.k-1H.sub.k.sup.T+R.sub.k wherein the adaptive threshold is =.Math..sub.a, wherein $=\frac{V_k^T{V}_k+\mathrm{Trace}\left(H_k{P}_{k|k-1}{H}_k^T+R_{k-1}\right)}{V_k^T{V}_k},$ .sub.a=(L.sub.k.sup.TL.sub.k(P.sub.k.sup.1+H.sub.k.sup.TH.sub.k).sup.1), Trace( ) represents a matrix trace operation, and ( ) represents a spectral radius of a matrix.

4. The up floating error correction method according to claim 3, wherein a calculation process of the H Kalman filter algorithm based on the adaptive multiple fading factor comprises: first, performing a time update, specifically comprising: predicting a state variable X.sub.k|k-1==.sub.k|k-1X.sub.k-1 of the current moment in one step according to an optimal state variable estimate X.sub.k-1 filtered and output at the previous moment, and predicting a state prediction covariance P.sub.k|k-1=S.sub.k.sub.k|k-1P.sub.k-1.sub.k|51.sup.TS.sub.k.sup.T+Q.sub.k-1 of the current moment according to both an optimal covariance estimation matrix P.sub.k-1 filtered and output at the previous moment and the adaptive multiple fading factor matrix S.sub.k output by a fading loop; second, performing a measurement update, specifically comprising: calculating the H filter gain matrix K.sub.k=P.sub.k|k-1H.sub.k.sup.T(H.sub.kP.sub.k|k-1H.sub.k.sup.T+I).sup.1 according to the state prediction covariance P.sub.k|k-1 of the current moment, updating a system optimal state variable estimate X.sub.k by using the H filter gain matrix K.sub.k and the state variable X.sub.k|k-1 predicted in one step after receiving a new external observation variable, and then, calculating the H filter state optimal covariance matrix $P_k=P_{k|k-1}-{}_{k|k-1}{P}_{k|k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k|k-1}{}_{k|k-1}^T$ after calculating an H threshold according to P.sub.k|k-1 and a time-varying factor .sub.a; and finally, performing a fading factor update, specifically comprising: calculating the adaptive multiple fading factor matrix S.sub.k=diag (s.sub.1, s.sub.2, s.sub.3 . . . , s.sub.n) after updating a transition matrix of the fading loop according to the H filter state optimal covariance matrix 1, unobservable variables.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a schematic diagram showing a structure of a navigation and positioning system for an underwater glider and a transition between underwater working and up floating error correction states in an embodiment of the present invention;

(2) FIG. 2 is a block diagram of a loose integrated navigation system for correction of GPS/INS location and velocity errors in an embodiment of the present invention; and

(3) FIG. 3 is a flowchart of an H Kalman filter algorithm based on an adaptive multiple fading factor for an up floating error correction state of an underwater glider in an embodiment of the present invention.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(4) The present invention is further described with reference to the accompanying drawings and specific embodiments:

(5) As shown in FIG. the present invention discloses a navigation and positioning system for an underwater glider, including a micro-electro-mechanical system inertial measurement unit (MEMS-IMU), a global positioning system (GPS) receiving module, a triaxial magnetometer, a Doppler velocimeter (DVL), a digital signal processing (DSP) module, and an advanced reduced instruction set microprocessor (ARM).

(6) The MEMS-IMU integrates a triaxial accelerometer and a triaxial gyroscope, and triaxial acceleration and angular rate information are output for obtaining navigation information including an attitude, a velocity, and a location of an underwater glider by using an inertial navigation algorithm. In addition, during up floating error correction, data fusion is performed on the navigation information and a GPS signal, to correct a navigation and positioning error.

(7) The triaxial magnetometer is used for a complementary filter algorithm, to correct a heading information error of the glider. In addition, if a heading variation is too large, reflecting strong magnetic interference, an up floating error correction operation needs to be performed.

(8) The DVL is configured to determine a motion state of the underwater glider. If a velocity variation is too large, reflecting that the glider is severely affected by ocean current surges, the up floating error correction operation also needs to be performed.

(9) The GPS receiving module is used for fusion with IMU data by using the designed Hoc Kalman filter algorithm based on an adaptive multiple fading factor during the up floating error correction operation of the glider, to correct velocity and location errors of the glider.

(10) The ARM and the DSP constitute an integrated navigation hardware processing system of the underwater glider. In an underwater working state, the ARM is configured to receive, process, and perform clock synchronization on signals from the magnetometer, the IMU, and the DVL. The DSP is used for calculation of a quaternion algorithm of inertial navigation. In an up floating error correction state, the ARM is configured to receive, process, and perform clock synchronization on signals from the GPS receiving module and the IMU. The DSP is configured to perform calculation of the H Kalman filter algorithm based on an adaptive multiple fading factor.

(11) The present invention discloses an up floating error correction method for a navigation and positioning system for an underwater glider, including the following steps:

(12) (1) Calculate an attitude, a velocity, and a location of an underwater glider by using a quaternion algorithm based on complementary filtering if a navigation and positioning system for the underwater glider starts a MEMS-IMU and a magnetometer module when the system is in an underwater working state. For a basic quaternion algorithm, refer to the book Inertial Navigation edited by Professor Qin Yongyuan, and for the complementary filter theory, refer to the invention patent written by Professor Chen Xiyuan, COMPLEMENTARY FILTERING-BASED UNDERWATER GLIDER ENERGY SAVING ALGORITHM.

(13) (2) Determine whether the underwater glider needs to perform an up floating error correction operation. Motion of the underwater glider is not intensive during a working process. If a heading variation is too large (strong magnetic interference), a velocity variation is too large (large ocean current surges), or it is manually determined that a navigation and positioning error of the glider is too large, the glider floats up to a water surface, to perform the up floating error correction operation.

(14) (3) Receive, after the glider floats up to the water surface, a longitude and latitude signal, an altitude signal, and a triaxial velocity signal from a GPS receiving module, and a triaxial acceleration signal and a triaxial angular velocity signal from an IMU by using an ARM+DSP integrated navigation hardware processing system. The DSP performs data fusion on the received multi-sensor signals. On the basis of ensuring reliability and precision of the MEMS-IMU and the GPS receiving module, a designed H Kalman filter algorithm based on an adaptive multiple fading factor is used. The algorithm is applied to a loose integrated navigation system for correction of GPS/INS location and velocity errors shown in FIG. 2, to perform data fusion. In the up floating error correction process of the glider, the triaxial acceleration information and the triaxial angular velocity information output by the IMU are calculated by an inertial navigation system to obtain a location, a velocity, and an attitude of the glider. The GPS system directly outputs the location and the velocity of the glider. Differences between information (location and velocity) output by the GPS system and information output by the INS system separately are obtained and input into an integrated navigation filter as external observation variables. With continuous iterations of the filter, optimal state variables (location and velocity errors) of the integrated navigation system are output for correcting a navigation location and a navigation velocity that include various noise and errors (an attitude error cannot be corrected in a loose integration mode).

(15) In the up floating error correction, a flowchart of an adopted H Kalman filter algorithm based on an adaptive multiple fading factor is shown in FIG. 3 and includes the following specific steps:

(16) {circle around (1)}. Establish an adaptive fading factor

(17) An integrated navigation system is considered as a linear dynamic system:
X.sub.k=.sub.k|k-1X.sub.k-1W.sub.k-1
Z.sub.k=H.sub.kx.sub.k+v.sub.k-1 where X.sub.k is a system state variable of a moment k, .sub.k|k-1 is a state transition matrix, and is system noise, Z.sub.k is a system observation variable of the moment k, H.sub.k is an observation matrix and v.sub.k-1 is observation noise. In addition, an expectation and a covariance of noise are as follows:
E(W.sub.k-1)=0, var(W.sub.k-1)=Q.sub.k-1
E(V.sub.k-1)=0, var(V.sub.k-1)=R.sub.k-1

(18) For a fading factor-based Kalman filter algorithm, refer to ADAPTIVE ESTIMATION OF MULTIPLE FADING FACTORS IN KALMAN FILTER FOR NAVIGATION APPLICATIONS by Yanrui Geng. The specific adaptive fading factor-based Kalman filter equation is written as:
X.sub.k=.sub.k|k-1X.sub.k-1
P.sub.k|k-1=S.sub.k.sub.k|k-1P.sub.k-1.sub.k|k-1.sup.TS.sub.k.sup.T+Q.sub.k-1
K.sub.k=P.sub.k|k-1H.sub.k.sup.T(H.sub.kP.sub.k|k-1H.sub.k.sup.T+R.sub.1
V.sub.k=Z.sub.kH.sub.kX.sub.k|k-1
X.sub.k=X.sub.k|k-1+K.sub.kV.sub.k where P.sub.k|k-1 is a state prediction covariance matrix, and K.sub.k is a gain matrix, where a gain factor is obtained by synthesizing the indeterminacy of a state space and the indeterminacy of the observation space, and the gain factor decides a correction capability of an external observation variable to the whole system. V.sub.k is an innovation matrix and represents a difference between an external observation variable Z.sub.k, and a state variable X.sub.k|k-1 of system prediction. Compared with the standard Kalman filter equation, only a fading factor matrix S.sub.k is added, for resolving the problem that when a mathematical model established for a research object cannot truly reflect an actual physical process and lacks knowledge of statistical characteristics of system noise, modeling does not match an obtained measured value, and it is likely to cause filter divergence. An adaptive fading factor matrix S.sub.k is obtained by using the following algorithm:

(19) $S_k=\mathrm{diag}\left(s_1,s_2,s_3.Math.,s_n\right)s_i=\{\begin{array}{cc}\max \left(1,\sqrt{\frac{{\left[{}_i\left(k\right)\right]}^2}{{}_i^2{j}_{\mathrm{ii}}\left(k\right){}_i}-\frac{b_{\mathrm{ii}}\left(k\right)}{j_{\mathrm{ii}}\left(k\right)}}\right),& \mathrm{observable}\mathrm{variables}\\ 1,& \mathrm{unobservable}\mathrm{variables}\end{array}$ where .sub.i is the i.sup.th observation element of the observation matrix H.sub.k, k.sub.ii (k) is the i.sup.th diagonalelement of the matrix J.sub.k=.sub.k|k-1P.sub.k-1.sub.k|k-1.sup.T, .sub.i is a threshold of Chi-square test, .sub.i (k) is the i.sup.th diagonal element of an innovation matrix V.sub.k, and b.sub.ii is the i.sup.th diagonal element of a matrix B.sub.k=H.sub.kQ.sub.k-1H.sub.k.sup.T+R.sub.k. It should be noted that fading factor calculation can only be performed on observable state variables of an observation equation. Otherwise, the observation equation degenerates into the standard Kalman filter equation.

(20) {circle around (2)}. Adaptive H Kalman filter algorithm

(21) A linear discrete system is considered:

(22) $\{\begin{array}{c}{x}_k={}_{k|k-1}{x}_{k-1}+w_{k-1}\\ y_k=L_k{x}_k\\ z_k=H_k{x}_k+v_{k-1}\end{array}$ where L.sub.k is an estimate of a linear combination of system state variables and is often taken as I, and Y.sub.k represents a direct estimate of the state variable X.sub.k.

(23) Therefore, a constructed construction cost function J is as follows:

(24) $J=\frac{\underset{k=1}{\overset{N}{.Math.}}{.Math.y_k-{\hat{y}}_k.Math.}^2}{{.Math.x_0-{\hat{x}}_0.Math.}_{P_0^{-1}}^2+\underset{k=1}{\overset{N}{.Math.}}\left({.Math.w_k.Math.}_{Q_k^{-1}}^2+{.Math.v_k.Math.}_{R_k^{-1}}^2\right)}$ where N is an overall filter time limit, X.sub.0 is an initial state variable of the system, and the remaining have been introduced. The central idea of H filtering is to ensure that an H norm of the construction cost function J is the minimum, to ensure that a maximum energy gain from an interference signal to an estimated error is the minimum. Therefore, impact of external disturbance and model indeterminacy on system output is minimized.
minJ.sub.=.sub.n

(25) However, because a closed solution to an H optimal estimation problem can only be obtained in some special cases, a design of H suboptimal filtering is usually considered. That is, a threshold that is sufficiently close to .sub.0 is given provided that an H norm of a constructed algebraic function is smaller than .

(26) In a case of full rank of .sub.k|k-1, sufficient and necessary conditions for existence of a solution to the H suboptimal problem can be given by using a Riccati inequality:
P.sub.k.sup.1+H.sub.k.sup.T.sup.2L.sub.k.sup.TL.sub.k>0 where P.sub.k is an optimal covariance estimate of a system state variable X.sub.k.

(27) It should be noted that robustness of a filter is related to a selected threshold . If the threshold is smaller and closer to .sub.0, robustness of the filter is stronger. However, should not be smaller than .sub.0. Otherwise, there is no solution to the H suboptimal problem, resulting in filter divergence. In addition, if approaches infinity, an H filter degenerates to a conventional Kalman filter. A conventional threshold is usually determined according to actual working experience of a glider and cannot be changed. As a result, filtering effects are more conservative. It cannot be guaranteed that an estimated error of a system is relatively small while having high robustness. Therefore, if a value of the threshold is adaptive to different up floating error correction environments of the underwater glider, navigation and positioning precision of the up floating error correction can be further improved while guaranteeing robustness. Therefore, an adaptive algorithm of the threshold is established according to the Riccati inequality as follows:
.sup.2>(L.sub.k.sup.TL.sub.k(P.sub.k.sup.1+H.sub.k.sup.TH.sub.k).sup.1) where ( ) represents a spectral radius of a matrix. If .sub.a=(L.sub.k.sup.TL.sub.k(P.sub.k.sup.1+H.sub.k.sup.TH.sub.k).sup.1), because an optimal state estimate covariance P.sub.k is continuously updated in the filter iterations, the optimal state estimate covariance P.sub.k is a time-varying factor.

(28) Ideally, an innovation matrix of Kalman filtering is V.sub.k, N (0, H.sub.kP.sub.k|k-1H.sub.k.sup.T+R.sub.k-1). However, impact of system indeterminacy leads to an abnormal observation variable, causing a filter to be out of order. Such a case may cause a variation of V.sub.k.sup.TV.sub.k of a sum of squares of an innovation sequence. When V.sub.k.sup.TV.sub.k is relatively large, reflecting that interference of indeterminacy has large impact on the system, in this case, robustness of the system should be increased (the threshold should be reduced). On the contrary, when V.sub.k.sup.TV.sub.k is relatively small, reflecting that the interference is relatively small, in this case, more attention should be paid to navigation and positioning precision of the system (the threshold should be increased). The adaptive threshold is: =.Math..sub.a,

(29) $\mathrm{where}=\frac{V_k^T{V}_k+\mathrm{Trace}+\left(H_k{P}_{k|k-1}{H}_k^T+R_{k-1}\right)}{V_k^T{V}_k}$ where Trace (H.sub.kP.sub.k|k-1H.sub.k.sup.T+R) is a trace operation, that is, a sum of diagonal elements of the matrix is obtained.

(30) Differences from a standard Kalman filter equation are as follows:

(31) $K_k=P_{k|k-1}{H_k^T\left(H_k{P}_{k|k-1}{H}_k^T+I\right)}^{-1}{P}_k=P_{k|k-1}-{}_{k|k-1}{P}_{k|k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k|k-1}{}_{k|k-1}^T\mathrm{where}{R}_{e,k}=\left[\begin{array}{cc}I& 0\\ 0& -{}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k|k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]$

(32) {circle around (3)}. H Kalman filter algorithm based on an adaptive multiple fading factor

(33) Algorithms in steps {circle around (1)} and {circle around (2)} are integrated to form an overall integrated navigation algorithm for up floating error correction of an underwater glider (an H Kalman filter algorithm based on an adaptive multiple fading factor). A flowchart of the algorithm is shown in FIG. 3. An integrated navigation filter is divided into three loops (a filter loop, a gain loop, and a fading loop) for calculation. A specific process is as follows:

(34) i) First, perform a time update: predict a state variable X.sub.k|k-1 of the current moment in one step according to an optimal state variable estimate X.sub.k-1 that is filtered and output at the previous moment, and predict a state prediction covariance P.sub.k|k-1 of the current moment according to both an optimal covariance estimation matrix P.sub.k-1 that is filtered and output at the previous moment and a fading factor matrix S.sub.k output by a fading loop.

(35) ii) Second, perform a measurement update: calculate a gain matrix K.sub.k according to the state prediction covariance P.sub.k|k-1 of the current moment, update a system optimal state variable estimate X.sub.k by using the gain matrix K.sup.k and the state variable X.sub.k|k-1 predicted in one step after receiving a new external observation variable, and then, calculate an H filter state optimal covariance P.sub.k after calculating an H threshold according to P.sub.k|k-1 and a time-varying factor .sub.a.

(36) iii) Finally, perform a fading factor update: calculate the adaptive multiple fading factor matrix S.sub.k after updating a transition matrix of the fading loop according to the H filter state optimal covariance P.sub.k.

(37) (4) Reduce, by using an H Kalman filter algorithm based on an adaptive multiple fading factor, velocity and location errors of the integrated navigation system gradually to approach zero. If the errors are smaller than specified thresholds, indicating that the up floating error correction is completed, the glider can switch to an underwater working state.