MOTION CONSTRAINT-AIDED UNDERWATER INTEGRATED NAVIGATION METHOD EMPLOYING IMPROVED SAGE-HUSA ADAPTIVE FILTERING

Abstract

A motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering includes establishing a Doppler log error model; constructing a state equation for an underwater integrated navigation system employing Kalman filtering; according to a relationship between a centripetal acceleration and a forward velocity of an underwater vehicle, establishing a constraint condition, and constructing a complete motion constraint model; establishing two measurement equations; and establishing a filter equation, conducting calculation by using a standard Kalman filtering algorithm when an underwater glider normally runs, and conducting time updating, measurement updating and filtering updating by using an improved Sage-Husa adaptive filtering algorithm when a measurement noise varies. The motion constraint-aided underwater integrated navigation method improves a filtering accuracy of the underwater integrated navigation system, restrains a filter divergence and has robustness and reliability.

Claims

1. A motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering, comprising the following steps of: step 1, establishing a Doppler log error model according to a working principle of a Doppler log; step 2, on a basis of an inertial navigation error equation, introducing a velocity error, a drift angle error and a proportionality coefficient error in the Doppler log error model to serve as state quantities of an underwater integrated navigation, and constructing a state equation for an underwater integrated navigation system employing Kalman filtering; step 3, establishing a carrier coordinate system, resolving a motion of an underwater vehicle into a plane perpendicular to Z axis and a plane perpendicular to X axis; and according to a relationship between a centripetal acceleration and a forward velocity of the underwater vehicle, establishing a constraint condition, and constructing a complete motion constraint model; step 4, establishing a first measurement equation according to navigation information of an inertial device and the Doppler log, and establishing a second measurement equation according to the complete motion constraint model in the step 2; and step 5, discretizing the state equation and the first measurement equation and the second measurement equation, establishing a filter equation in combination with step 2 and step 4, conducting calculation by using a standard Kalman filtering algorithm when an underwater glider normally runs, and conducting time updating, measurement updating and filtering updating by using an improved Sage-Husa adaptive filtering algorithm when measurement noise varies.

2. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering according to claim 1, wherein the working principle of the Doppler log in the step 1 is as follows: two pairs of transducers in a front, a back, a left and a right are mounted at a bottom end of the underwater vehicle to transmit wave beams in four directions respectively, Doppler shifts are obtained by measuring frequencies of transmitting wave beams and frequencies of the wave beams after being reflected, and then velocities of the underwater vehicle in three directions in the carrier coordinate system are obtained as follows: $v_x^b=\frac{c}{4f_0\cos \alpha }{f}_{d13};$ $v_y^b=\frac{c}{4f_0\cos \alpha }{f}_{d24};$ $v_z^b=\frac{c}{8f_0\cos \alpha }\left(f_{d1}+f_{d2}+f_{d3}+f_{d4}\right);$ $f_{d13}=f_{d1}-f_{d3},f_{d24}=f_{d2}-f_{d4};$ the Doppler log error model is as follows: $\{\begin{array}{c}\delta v_{dU}=\delta v_d\sin \beta +\delta C{v}_d\sin \beta \\ \delta v_{dE}=v_d\cos K_d\left(\gamma +\mathrm{\delta \Delta }\right)\cos \beta +\delta {\mathrm{Cv}}_d\sin K_d\cos \beta +\delta v_d\sin K_d\cos \beta \\ \delta v_{dN}=-v_d\sin K_d\left(\gamma +\mathrm{\delta \Delta }\right)\cos \beta +\delta {\mathrm{Cv}}_d\cos K_d\cos \beta +\delta v_d\cos K_d\cos \beta \end{array};$ wherein v.sub.d is the forward velocity of the underwater vehicle; c is a light velocity; v.sub.x.sup.b, v.sub.y.sup.b and v.sub.z.sup.b are velocities of the underwater vehicle in the carrier coordinate system respectively; f.sub.0 is the frequency of the transmitting wave beams; f.sub.d1, f.sub.d2, f.sub.d3 and f.sub.d4 represent the Doppler shifts; α is a tilt angle of the transmitting wave beams; δv.sub.dU, δv.sub.dE and δv.sub.dN are velocity errors of the underwater vehicle in three directions in an East-North-Up coordinate system respectively; δv.sub.d is an error on a Doppler velocity measurement; β is a pitch angle of the underwater vehicle; δC is an error on a proportionality coefficient; and K.sub.d, γ and δΔ are errors on a track considering a drift angle, an azimuth misalignment angle and the drift angle of the underwater vehicle respectively.

3. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering according to claim 1, wherein the state equation for the underwater integrated navigation system employing the Kalman filtering in the step 2 is as follows:
X.sub.k=A.sub.k,k−1X.sub.k−1+Γ.sub.k,k−1W.sub.k−1; wherein A.sub.k,k−1 represents a state transition matrix of the underwater integrated navigation system from a time k−1 to a time k, Γ.sub.k,k−1 represents a noise driving matrix of the underwater integrated navigation system, and W.sub.k−1 is a noise excitation sequence of the underwater integrated navigation system; a corresponding state vector is as follows:
X=[δLδλδhδv.sub.Eδv.sub.Nδv.sub.Uαβγε.sub.xε.sub.yε.sub.z∇.sub.x∇.sub.y∇.sub.zδv.sub.dδΔδC]; wherein δv.sub.d, δΔ and δC are represented by following equations: $\{\begin{array}{c}\delta {\stackrel{.}{v}}_d=-{\beta }_d\delta v_d+{\omega }_d\\ \delta \stackrel{.}{\Delta }=-{\beta }_{\Delta }\delta \Delta +{\omega }_{\Delta }\\ \delta \stackrel{.}{C}=0\end{array};$ in the equations, δL, δλ and δh represent errors on a longitude, a latitude and a height of a carrier respectively; δv.sub.E, δv.sub.N and δv.sub.U are velocity errors of the carrier in a east direction, a north direction and an up direction respectively; α, β and γ are errors on an attitude angle of the carrier respectively; ε.sub.x, ε.sub.y and ε.sub.y are zero biases of a gyroscope; ∇.sub.x, ∇.sub.y and ∇.sub.z represent accelerometer zero biases; β.sub.d.sup.−1 and β.sub.Δ.sup.−1 are a time related to a velocity offset error and a time related to the drift angle error respectively; and ω.sub.d and ω.sub.Δ are both excitation white noise.

4. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering according to claim 1, wherein the step 3 specifically comprises the following process: assuming that the velocities, in two directions perpendicular to the forward velocity, of the underwater vehicle are related to a flowing velocity of seawater only and the flowing velocity is 0, the constraint condition is obtained as follows: $\{\begin{array}{c}{v}_x^b=0\\ v_z^b=0\end{array};$ the motion of the underwater vehicle is resolved into the plane perpendicular to the Z axis and the plane perpendicular to the X axis, and following equations are obtained according to a kinematics equation:
a.sub.rx=v.sub.y.sup.bw.sub.nbz.sup.b;
a.sub.rz=v.sub.y.sup.bw.sub.nbx.sup.b; following equations are obtained according to the inertial device: $\{\begin{array}{c}{a}_{rx}=f_x^b+{\left(C_n^b{g}^n\right)}_1\\ a_{rz}=-f_z^b+{\left(C_n^b{g}^n\right)}_3\\ v_y^b={\left(C_n^b{v}^n\right)}_2\\ w_{nbz}^b=w_{ibz}^b-{\left(C_n^b\left(w_{ie}^n+w_{en}^n\right)\right)}_3\\ w_{nbx}^b=w_{ibx}^b-{\left(C_n^b\left(w_{ie}^n+w_{en}^n\right)\right)}_1\end{array};$ wherein the complete motion constraint model is obtained: $\{\begin{array}{c}{v}_x^b=0\\ v_z^b=0\\ a_{0x}=a_{rx}-v_y^b{w}_{nbz}^b\\ a_{0z}=a_{rz}-v_z^b{w}_{nbx}^b\end{array};$ and then an obtained error model for a complete motion constraint is as follows: $\{\begin{array}{c}\delta v_x^b={\left(C_n^b\delta v^n\right)}_1-{\left(C_n^b{\varphi }^n\times v^n\right)}_1\\ \delta v_y^b={\left(C_n^b\delta v^n\right)}_3-{\left(C_n^b{\varphi }^n\times v^n\right)}_3\\ \delta a_{0x}={\left(C_n^b{\varphi }^n\times v^n\right)}_2{\omega }_{ibz}^b-{\left(C_n^b{\varphi }^n\times g^n\right)}_1-{\left(C_n^b\delta v^n\right)}_2{\omega }_{ibz}^b+{\nabla }_x^b-{\left(C_n^b{v}^n\right)}_2{\epsilon }_z^b\\ \delta a_{0z}={\left(C_n^b{\varphi }^n\times v^n\right)}_2{\omega }_{ibx}^b-{\left(C_n^b{\varphi }^n\times g^n\right)}_3-{\left(C_n^b\delta v^n\right)}_2{\omega }_{ibx}^b-{\nabla }_z^b-{\left(C_n^b{v}^n\right)}_2{\epsilon }_x^b\end{array};$ wherein C.sub.n.sup.b is a transition matrix from a navigation coordinate system to the carrier coordinate system; ∅.sup.n represents an attitude angle in the navigation coordinate system; v.sup.n is a velocity of the underwater vehicle in the navigation coordinate system; g.sup.n is a gravitational acceleration in the navigation coordinate system; ε.sub.x.sup.b and ε.sub.z.sup.b are zero biases of a gyroscope in the X axis and the Z axis in the carrier coordinate system; a.sub.rx and a.sub.rz are centripetal acceleration values of the underwater vehicle in an X direction and a Z direction in the carrier coordinate system; w.sub.ibz.sup.b and w.sub.ibz.sup.b are sensitive angular velocities of the inertial device in the X axis and the Z axis respectively; f.sub.x.sup.b and f.sub.z.sup.b and are specific force values of an accelerometer in the X axis and the Z axis respectively; and w.sub.ie.sup.n and w.sub.en.sup.n are a rotation angular velocity of the earth and an angular velocity caused by a motion of a carrier respectively.

5. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering according to claim 1, wherein the first measurement equation in the step 4 is as follows: $Z_1=\left[\begin{array}{c}{v}_E-v_{\mathrm{dE}}\\ v_N-v_{\mathrm{dN}}\\ v_U-v_{\mathrm{dU}}\end{array}\right];$ and the second measurement equation is as follows: $Z_2=\left[\begin{array}{c}{v}_x^b\\ v_z^b\\ \delta a_{0x}\\ \delta a_{0z}\end{array}\right];$ wherein v.sub.E, v.sub.N and v.sub.U are velocities, obtained by an inertial navigation calculation, of the underwater vehicle in an east direction, a north direction and an up direction respectively; v.sub.dE, v.sub.dN and v.sub.dU are velocities, measured by the Doppler log and subjected to a coordinate transformation, in the east direction, the north direction and the up direction respectively; a measurement Z.sub.1 is a difference between the velocities obtained by the inertial navigation calculation and the velocities measured by the Doppler log; and a measurement Z.sub.2 is velocity constraints of the underwater vehicle in two directions and acceleration constraints when a carrier moves.

6. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering according to claim 1, wherein a state space model in the step 5 comprises:
X.sub.k=A.sub.k,k−1X.sub.k−1+Γ.sub.k,k−1W.sub.k−1;
Z.sub.k=H.sub.kX.sub.k+V.sub.k; wherein Z.sub.k is an observation vector of a sensor at a time k; H is a transition matrix of the underwater integrated navigation system from a state space to an observation space; V.sub.k is a measurement noise sequence; a state transition matrix A of the underwater integrated navigation system is as follows: $A=\left[\begin{array}{cccc}{A}_{{\mathrm{SINS}}_{9\times 9}}& 0_{9\times 3}& 0_{9\times 3}& 0_{9\times 3}\\ 0_{3\times 9}& A_{{\mathrm{Gyro}}_{3\times 3}}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 9}& 0_{3\times 3}& A_{{\mathrm{Acc}}_{3\times 3}}& 0_{3\times 3}\\ 0_{3\times 9}& 0_{3\times 3}& 0_{3\times 3}& A_{{\mathrm{DVL}}_{3\times 3}}\end{array}\right];$ wherein A.sub.SINS.sub.9×9, A.sub.Gyro.sub.3×3, A.sub.Acc.sub.3×3 and A.sub.DVI.sub.3×3 transition matrixes of an inertial navigation system, a gyroscopic drift, an inverse correlation time matrix of an accelerometer error as well as an inverse correlation matrix of the Doppler log error respectively; a measurement matrix of the underwater integrated navigation system is as follows:
H.sub.1=[0.sub.3×3I.sub.3×3S.sub.10.sub.3×6S.sub.2]; wherein: $S_1=\left[\begin{array}{ccc}-v_N& 0& 0\\ -v_E& 0& 0\\ 0& 0& 0\end{array}\right],S_1=\left[\begin{array}{ccc}-\sin K_d\cos \beta & -v_N& -v_E\\ -\cos K_d\cos \beta & v_E& -v_N\\ -\sin \beta & 0& v_d\sin \beta \end{array}\right];$ and
H.sub.2=[0.sub.3×4M.sub.1M.sub.2M.sub.3M.sub.40.sub.3×4]; assuming: $C_n^b=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right];$ then: $M_1=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{31}& T_{33}& T_{33}\\ -T_{21}{\omega }_{ibz}^b& -T_{22}{\omega }_{ibz}^b& -T_{23}{\omega }_{ibz}^b\\ -T_{21}{\omega }_{ibx}^b& -T_{22}{\omega }_{ibx}^b& -T_{23}{\omega }_{ibx}^b\end{array}\right];$ $M_2=\left[\begin{array}{ccc}-\left(T_{11}{v}_U-T_{13}{v}_E\right)& -\left(T_{13}{v}_N-T_{12}{v}_U\right)& -\left(T_{12}{v}_E-T_{11}{v}_N\right)\\ -\left(T_{31}{v}_U-T_{33}{v}_E\right)& -\left(T_{33}{v}_N-T_{32}{v}_U\right)& -\left(T_{32}{v}_E-T_{31}{v}_N\right)\\ T_{31}g-T_{23}{v}_E{\omega }_{ibx}^b& -T_{32}g+T_{23}{v}_N{\omega }_{ibx}^b& T_{22}{v}_E{\omega }_{ibx}^b-T_{21}{v}_N{\omega }_{ibx}^b\\ T_{11}g-T_{23}{v}_E{\omega }_{ibz}^b& -T_{12}g+T_{23}{v}_N{\omega }_{ibz}^b& T_{22}{v}_E{\omega }_{ibx}^b-T_{21}{v}_N{\omega }_{ibz}^b\end{array}\right];$ $M_3=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ -{\left(C_n^b{v}_n\right)}_2& 0& 0\\ 0& 0& -{\left(C_n^b{v}_n\right)}_2\end{array}\right],M_4=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& -1\\ 1& 0& 0\end{array}\right];$ and wherein: $\{\begin{array}{cc}E\left[W_k\right]=0& E\left[W_k{W}_j^T\right]=Q_k{\delta }_{kj}\\ E\left[V_k\right]=0& E\left[V_k{V}_j^T\right]=R_k{\delta }_{kj}\\ E\left[W_k{V}_k^T\right]=0& \end{array}.$

7. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering according to claim 1, wherein a calculation process of the improved Sage-Husa adaptive filtering algorithm in the step 5 is as follows: $X_k=X_{k,k-1}+K_k\left(Z_k-H_k{X}_{k,k-1}\right);$ $K_k=P_{k,k-1}{H_k^T\left(H_k{P}_{k,k-1}{H}_k^T+R_k\right)}^{-1};$ ${\hat{R}}_k=\left(1-{\beta }_k\right){\hat{R}}_{k-1}+{\beta }_k\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{K/K-1}{H}_k^T\right);$ ${\beta }_k=\{\begin{array}{cc}\frac{{\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}-tr\left(H_k{P}_{k/k-1}{H}_k^T\right)-tr\left({\hat{R}}_{k-1}\right)}{tr\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T\right)-tr\left({\hat{R}}_{k-1}\right)}& {\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}>tr\left[E\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T\right)\right]\\ 1& {\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}\le tr\left[E\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T\right)\right]\end{array};$ $P_{k,k-1}=A_{k,k-1}{P}_{k-1}{A}_{k,k-1}^T+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^T;$ $P_k=\left(I-K_k{H}_k\right)P_{k,k-1};$ wherein X.sub.k represents a state variable of a carrier at a time k; A represents a state transition matrix of the underwater integrated navigation system from the time k to a time k+1; Z.sub.k is an observation vector of a sensor at the time k; H is a transition matrix of the underwater integrated navigation system from a state space to an observation space; K.sub.k is a Kalman filtering gain at the time k; Q is a noise covariance matrix of the underwater integrated navigation system; R is an observation covariance matrix; and P is an error covariance matrix.

8. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering according to claim 7, wherein the β.sub.k is deduced through the following process: in the Kalman filtering, equations for measuring prediction errors are as follows: $\begin{array}{c}{\tilde{Z}}_{k/k-1}=Z_k-{\hat{Z}}_{k/k-1}\\ =H_k{\tilde{X}}_{k/k-1}+V_k-H_k{\hat{X}}_{k/k-1}\\ =H_k{\tilde{X}}_{k/k-1}+V_k\end{array};$ variances are calculated at two sides at a same time to obtain:
E[{tilde over (Z)}.sub.k/k−1{tilde over (Z)}.sub.k/k−1.sup.T]=H.sub.kP.sub.k/k−1H.sub.k.sup.T+R.sub.k; by using a method of exponentially decreasing a weighted average, an equation is obtained:
{circumflex over (R)}.sub.k=(1−β.sub.k){circumflex over (R)}.sub.k−1+β.sub.k({tilde over (Z)}.sub.k/k−1{tilde over (Z)}.sub.k/k−1.sup.T−H.sub.kP.sub.K/K−1H.sub.k.sup.T); whether a filter divergence occurs or not is judged; a general filtering is conducted if a filtering is normal, and when the filter divergence is detected, an optimal β.sub.k is calculated in real time, and the filter divergence is prevented; and whether the filter divergence occurs or not is judged according to a following equation:
{tilde over (Z)}.sub.k/k−1.sup.T{tilde over (Z)}.sub.k/k−1>γ.Math.tr[E({tilde over (Z)}.sub.k/k−1{tilde over (Z)}.sub.k/k−1.sup.T)]; and if the above equation holds, the filter divergence is indicated, wherein γ is a reserve coefficient; and when γ=1, a convergent criterion is the most rigorous, and a following equation is obtained by employing a most rigorous convergent criterion: ${\beta }_k=\frac{{\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}-tr\left(H_k{P}_{k/k-1}{H}_k^T\right)-tr\left({\hat{R}}_{k-1}\right)}{tr\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T\right)-tr\left({\hat{R}}_{k-1}\right)}.$

9. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering according to claim 1, wherein in the step 5, a case that the measurement noise varies comprises at least one of the following situations: the underwater glider encounters an obstacle or makes a maneuvering turning.

10. The motion constraint-aided underwater integrated navigation method according to claim 1, wherein an obstacle comprises an underwater ditch and a fish school.

Description

BRIEF DESCRIPTION OF DRAWINGS

[0053] FIG. 1 is a working principle diagram of a Doppler log.

[0054] FIG. 2 is a schematic diagram of a Doppler log error model.

[0055] FIG. 3 is a schematic diagram of resolving motion of an underwater vehicle.

[0056] FIG. 4 is a flow chart of a motion constraint-aided algorithm employing improved Sage-Husa adaptive filtering.

[0057] FIG. 5 are diagrams of position and velocity errors of a navigation system with a standard Kalman filtering algorithm.

[0058] FIG. 6 are diagrams of position and velocity errors of a navigation system with a motion constraint-aided algorithm employing an improved Sage-Husa adaptive filtering.

[0059] FIG. 7 are diagrams of position and velocity errors of a navigation system with a motion constraint-aided Sage-Husa adaptive algorithm when measurement noise is interfered.

[0060] FIG. 8 are diagrams of position and velocity errors of a navigation system with a motion constraint-aided improved Sage-Husa adaptive algorithm when measurement noise is interfered.

DETAILED DESCRIPTION OF THE EMBODIMENTS

[0061] The technical solution provided by the present invention will be described in detail below in combination with specific embodiments. It should be understood that these implementations are used for construing the present invention only, but not limiting its scope.

[0062] The present invention proposes a motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering. The implementation principle and the method process are shown in FIGS. 1-4. The motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering specifically includes the following steps:

[0063] step 1, a Doppler log error model is established according to the working principle of a Doppler log:

[0064] the principle of the Doppler log is relatively simple, as shown in FIG. 1; two pairs of transducers in the front, back, left and right are mounted at the bottom end of the underwater vehicle to transmit wave beams in four directions respectively, Doppler shifts are obtained by measuring frequencies of transmitting wave beams and frequencies of the wave beams after being reflected, and then velocities of the underwater vehicle in three directions in the carrier coordinate system are obtained as follows:

[00018]$v_x^b=\frac{c}{4f_0\cos \alpha }{f}_{d13}{v}_y^b=\frac{c}{4f_0\cos \alpha }{f}_{d24}{v}_z^b=\frac{c}{8f_0\cos \alpha }\left(f_{d1}+f_{d2}+f_{d3}+f_{d4}\right)f_{d13}=f_{d1}-f_{d3},f_{d24}=f_{d2}-f_{d4}$

[0065] and correspondingly, the obtained Doppler log error model is as follows:

[00019]$\{\begin{array}{c}\delta v_{\mathrm{dU}}=\delta v_d\sin \beta +\delta {\mathrm{Cv}}_d\sin \beta \\ \delta v_{\mathrm{dE}}=v_d\cos K_d\left(\gamma +\delta \Delta \right)\cos \beta +\delta {\mathrm{Cv}}_d\sin K_d\cos \beta +\delta v_d\sin K_d\cos \beta \\ \delta v_{\mathrm{dN}}=-v_d\sin K_d\left(\gamma +\delta \Delta \right)\cos \beta +\delta {\mathrm{Cv}}_d\cos K_d\cos \beta +\delta v_d\cos K_d\cos \beta \end{array}$

[0066] where v.sub.d is the forward velocity of the underwater vehicle; c is the light velocity; v.sub.x.sup.b, v.sub.y.sup.b and v.sub.z.sup.b are velocities of the underwater vehicle in the carrier coordinate system respectively; f.sub.0 is the frequency of transmitting wave; f.sub.d1, f.sub.d2, f.sub.d3 and f.sub.d4 represent the Doppler shifts; α is a tilt angle of the transmitting wave beams; δv.sub.dU, δv.sub.dE and δv.sub.dN are velocity errors of the underwater vehicle in three directions in the East-North-Up coordinate system respectively; δv.sub.d is an error on Doppler velocity measurement; β is a pitch angle of the underwater vehicle; δC is an error on proportionality coefficient; and K.sub.d, γ and δΔ are errors on track considering a drift angle, an azimuth misalignment angle and the drift angle of the underwater vehicle respectively.

[0067] Step 2, on the basis of an inertial navigation error equation, a velocity error δv.sub.d, a drift angle error δΔ and a proportionality coefficient error δC in the Doppler log error model are introduced to serve as state quantities of underwater integrated navigation, as shown in FIG. 2; and a state equation for an underwater integrated navigation system employing Kalman filtering is constructed:

[0068] where the state equation for the motion constraint-aided underwater combined navigation method employing improved Sage-Husa adaptive filtering algorithm:

X.sub.k=A.sub.k,k−1X.sub.k−1+Γ.sub.k,k−1W.sub.k−1

[0069] where A.sub.k,k−1 represents a state transition matrix of the system from a time k−1 to a time k, Γ.sub.k,k−1 represents a noise driving matrix of the system, and W.sub.k−1 is a noise excitation sequence of the system. a corresponding state vector is as follows:

X=[δLδλδhδv.sub.Eδv.sub.Nδv.sub.Uαβγε.sub.xε.sub.yε.sub.z∇.sub.x∇.sub.y∇.sub.zδv.sub.dδΔδC]

[0070] where δv.sub.d, δΔ and δC are represented by following equations:

[00020]$\{\begin{array}{c}\delta {\stackrel{.}{v}}_d=-{\beta }_d\delta v_d+{\omega }_d\\ \delta \stackrel{.}{\Delta }=-{\beta }_{\Delta }\delta \Delta +{\omega }_{\Delta }\\ \delta \stackrel{.}{C}=0\end{array}$

[0071] and in the equations, δL, δλ and δh represent errors on longitude, latitude and height of a carrier respectively; δv.sub.E, δv.sub.N and δv.sub.U are velocity errors of the carrier in the east direction, the north direction and the up direction respectively; α, β and γ are errors on an attitude angle of the carrier respectively; ε.sub.x, ε.sub.y and ε.sub.y are zero biases of a gyroscope; ∇.sub.x, ∇.sub.y and ∇.sub.z represent accelerometer zero biases; β.sub.d.sup.−1 and β.sub.Δ.sup.−1 are a time related to a velocity offset error and a time related to the drift angle error respectively; and ω.sub.d and ω.sub.Δ are both excitation white noise.

[0072] Step 3, any motion of the underwater vehicle may be resolved into two planes perpendicular to each other, specifically, a carrier coordinate system is established, and the motion of the underwater vehicle is resolved into a plane perpendicular to Z axis and a plane perpendicular to X axis, as shown in FIG. 3; and according to a relationship between a centripetal acceleration and a forward velocity of the underwater vehicle, a constraint condition is established, and a complete motion constraint model is constructed:

[0073] As a water flow rate at a certain depth at the bottom of the sea is relatively stable, assuming that velocities, in two directions perpendicular to the forward velocity, of the underwater vehicle are related to a flowing velocity of seawater only and that the flowing velocity is 0, the constraint condition may be obtained as follows:

[00021]$\{\begin{array}{c}{v}_x^b=0\\ v_z^b=0\end{array}$

[0074] the constraint condition can constrain the velocities of the underwater vehicle in the two directions only, whereas the centripetal acceleration can be generated when the underwater vehicle turns or makes a strong maneuvering motion. Following equations may be obtained according to a kinematics equation:

a.sub.rx=v.sub.y.sup.bw.sub.nbz.sup.b

a.sub.rz=v.sub.y.sup.bw.sub.nbx.sup.b

[0075] following equations may be obtained according to the inertial device:

[00022]$\{\begin{array}{c}{a}_{\mathrm{rx}}=f_x^b+{\left(C_n^b{g}^n\right)}_1\\ a_{\mathrm{rz}}=-f_z^b+{\left(C_n^b{g}^n\right)}_3\\ v_y^b={\left(C_n^b{v}^n\right)}_2\\ w_{\mathrm{nbz}}^b=w_{\mathrm{ibz}}^b-{\left(C_n^b\left(w_{\mathrm{ie}}^n+w_{\mathrm{en}}^n\right)\right)}_3\\ w_{\mathrm{nbx}}^b=w_{\mathrm{ibx}}^b-{\left(C_n^b\left(w_{\mathrm{ie}}^n+w_{\mathrm{en}}^n\right)\right)}_1\end{array}$

[0076] so that the complete motion constraint model may be obtained:

[00023]$\{\begin{array}{c}{v}_x^b=0\\ v_z^b=0\\ a_{0x}=a_{rx}-v_y^b{w}_{nbz}^b\\ a_{0z}=a_{rz}-v_z^b{w}_{nbx}^b\end{array}$

[0077] and then an obtained error model for a complete motion constraint is as follows:

[00024]$\{\begin{array}{c}\delta v_x^b={\left(C_n^b\delta v^n\right)}_1={\left(C_n^b{\varphi }^n\times v^n\right)}_1\\ \delta v_y^b={\left(C_n^b\delta v^n\right)}_3-{\left(C_n^b{\varphi }^n\times v^n\right)}_3\\ \delta a_{0x}={\left(C_n^b{\varphi }^n\times v^n\right)}_2{\omega }_{\mathrm{ibz}}^b-{\left(C_n^b{\varphi }^n\times g^n\right)}_1-{\left(C_n^b\delta v^n\right)}_2{\omega }_{\mathrm{ibz}}^b+{▽}_x^b-{\left(C_n^b{v}^n\right)}_2{\epsilon }_z^b\\ \delta a_{0z}={\left(C_n^b{\varphi }^n\times v^n\right)}_2{\omega }_{\mathrm{ibx}}^b-{\left(C_n^b{\varphi }^n\times g^n\right)}_3-{\left(C_n^b\delta v^n\right)}_2{\omega }_{\mathrm{ibx}}^b-{▽}_z^b-{\left(C_n^b{v}^n\right)}_2{\epsilon }_x^b\end{array}$

[0078] where C.sub.n.sup.b is a transition matrix from a navigation coordinate system to the carrier coordinate system; ∅.sup.n represents an attitude angle in the navigation coordinate system; v.sup.n is a velocity of the underwater vehicle in the navigation coordinate system; g.sup.n is a gravitational acceleration in the navigation coordinate system; ε.sub.x.sup.b and ε.sub.z.sup.b are zero biases of the gyroscope in the X axis and the Z axis in the carrier coordinate system; a.sub.rx and a.sub.rz are centripetal acceleration values of the underwater vehicle in an X direction and a Z direction in the carrier coordinate system; w.sub.ibx.sup.b and w.sub.ibz.sup.b are sensitive angular velocities of the inertial device in the X axis and the Z axis respectively; f.sub.x.sup.b and f.sub.z.sup.b are specific force values of an accelerometer in the X axis and the Z axis respectively; and w.sub.ie.sup.n and w.sub.en.sup.n are a rotation angular velocity of the earth and an angular velocity caused by motion of the carrier respectively.

[0079] Step 4, a measurement equation (1) is established according to navigation information of an inertial device and the Doppler log, and a measurement equation (2) is established according to the complete motion constraint model:

[00025]$\begin{array}{cc}{Z}_1=\left[\begin{array}{c}{v}_E-v_{dE}\\ v_N-v_{dN}\\ v_U-v_{dU}\end{array}\right]& \left(1\right)\end{array}$$\begin{array}{cc}{Z}_2=\left[\begin{array}{c}{v}_x^b\\ v_z^b\\ \delta a_{0x}\\ \delta a_{0z}\end{array}\right]& \left(2\right)\end{array}$

[0080] In the equations, v.sub.E, v.sub.N and v.sub.U are velocities, obtained by inertial navigation calculation, of the underwater vehicle in the east direction, the north direction and the up direction (the present invention selects the East-North-Up coordinate system as the navigation coordinate system) respectively; v.sub.dE, v.sub.dN and v.sub.dU are velocities, measured by the Doppler log and subjected to coordinate transformation, in the east direction, the north direction and the up direction respectively; a measurement Z.sub.1 is a difference between the velocities obtained by inertial navigation calculation and the velocities measured by the Doppler log; and a measurement Z.sub.2 is velocity constraints of the underwater vehicle in two directions and acceleration constraints when the carrier moves. When the underwater vehicle normally works, the measurement Z.sub.2 should take a value of 0, or the measurement noise should be white noise.

[0081] Step 5, the state equation and the measurement equations are discretized, a filter equation is established in combination with step 2 and step 4; calculation is conducted by using a standard Kalman filtering algorithm when an underwater glider normally runs; and when the underwater glider encounters an underwater ditch or fish school or makes a strong maneuvering turning, and measurement noise varies, time updating, measurement updating and filtering updating are conducted by using an improved Sage-Husa adaptive filtering algorithm. The specific process is shown in FIG. 4:

[0082] in combination with steps 1-4, a state space model of the system includes a state equation and measurement equations which are specifically as follows:

X.sub.k=A.sub.k,k−1X.sub.k−1+Γ.sub.k,k−1W.sub.k−1

Z.sub.k'H.sub.kX.sub.k+V.sub.k

[0083] where Z.sub.k is an observation vector of a sensor at the time k; H is a transition matrix of the system from a state space to an observation space; and V.sub.k is a measurement noise sequence:

[0084] a state transition matrix A of the system is as follows:

[00026]$A=\left[\begin{array}{cccc}{A}_{{\mathrm{SINS}}_{9\times 9}}& 0_{9\times 3}& 0_{9\times 3}& 0_{9\times 3}\\ 0_{3\times 9}& A_{Gyr{o}_{3\times 3}}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 9}& 0_{3\times 3}& A_{Ac{c}_{3\times 3}}& 0_{3\times 3}\\ 0_{3\times 9}& 0_{3\times 3}& 0_{3\times 3}& A_{DV{L}_{3\times 3}}\end{array}\right]$

[0085] where A.sub.SINS.sub.9×9, A.sub.Gyro.sub.3×3, A.sub.Acc.sub.3×3 and A.sub.DVL.sub.3×3 represent the state transition matrix of the inertial navigation system, a gyroscopic drift, an inverse correlation time matrix of an accelerometer error as well as an inverse correlation matrix of the Doppler log error respectively.

[0086] a measurement matrix of the system is as follows:

H.sub.1=[0.sub.3×3I.sub.3×S.sub.10.sub.3×6S.sub.2]

[0087] where:

[00027]$S_1=\left[\begin{array}{ccc}-v_N& 0& 0\\ -v_E& 0& 0\\ 0& 0& 0\end{array}\right],S_1=\left[\begin{array}{ccc}-s\mathrm{in}{K}_d\cos \beta & -v_N& -v_E\\ -\cos K_d\cos \beta & v_E& -v_N\\ -s\mathrm{in}\beta & 0& v_d\sin \beta \end{array}\right];$

and

H.sub.2=[0.sub.3×4M.sub.1M.sub.2M.sub.3M.sub.40.sub.3×4]

[0088] assuming that:

[00028]$C_n^b=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

[0089] then:

[00029]$M_1=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{31}& T_{33}& T_{33}\\ -T_{21}^b{\omega }_{\mathrm{ibz}}^b& -T_{22}{\omega }_{\mathrm{ibz}}^b& -T_{23}{\omega }_{\mathrm{ibz}}^b\\ -T_{21}^b{\omega }_{\mathrm{ibx}}^b& -T_{22}{\omega }_{\mathrm{ibx}}^b& -T_{23}{\omega }_{\mathrm{ibx}}^b\end{array}\right]$$M_2=\left[\begin{array}{ccc}-\left(T_{11}{v}_U-T_{13}{v}_E\right)& -\left(T_{13}{v}_N-T_{12}{v}_U\right)& -\left(T_{12}{v}_E-T_{11}{v}_N\right)\\ -\left(T_{31}{v}_U-T_{33}{v}_E\right)& -\left(T_{33}{v}_N-T_{32}{v}_U\right)& -\left(T_{32}{v}_E-T_{31}{v}_N\right)\\ T_{31}g-T_{23}{v}_E{\omega }_{\mathrm{ibx}}^b& -T_{32}g+T_{23}{v}_N{\omega }_{\mathrm{ibx}}^b& T_{22}{v}_E{\omega }_{\mathrm{ibx}}^b-T_{21}{v}_N{\omega }_{\mathrm{ibx}}^b\\ T_{11}g-T_{23}{v}_E{\omega }_{\mathrm{ibz}}^b& -T_{12}g+T_{23}{v}_N{\omega }_{\mathrm{ibz}}^b& T_{22}{v}_E{\omega }_{\mathrm{ibx}}^b-T_{21}{v}_N{\omega }_{\mathrm{ibz}}^b\end{array}\right]$$M_3=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ -{\left(C_n^b{v}_n\right)}_2& 0& 0\\ 0& 0& -{\left(C_n^b{v}_n\right)}_2\end{array}\right],M_4=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& -1\\ 1& 0& 0\end{array}\right];$

and

[0090] where:

[00030]$\{\begin{array}{cc}E\left[W_k\right]=0& E\left[W_k{W}_j^T\right]=Q_k{\delta }_{\mathrm{kj}}\\ E\left[V_k\right]=0& E\left[V_k{V}_j^T\right]=R_k{\delta }_{\mathrm{kj}}\\ E\left[W_k{V}_k^T\right]=0& \end{array}$

[0091] in the underwater integrated navigation system, system state noise is relatively stable usually, and therefore, the present invention only conducts adaptive filtering on the measurement noise. in Kalman filtering, equations for measuring prediction errors are as follows:

[00031]${\tilde{Z}}_{k/k-1}=Z_k-{\hat{Z}}_{k/k-1}=H_k{\tilde{X}}_{k/k-1}+V_k-H_k{\hat{X}}_{k/k-1}=H_k{\tilde{X}}_{k/k-1}+V_k$

[0092] variances are calculated at the two sides at the same time to obtain:

E[{tilde over (Z)}.sub.k/k−1{tilde over (Z)}.sub.k/k−1.sup.T]=H.sub.kP.sub.k/k−1H.sub.k.sup.T+R.sub.k

[0093] by using a method of exponentially decreasing a weighted average, an equation may be obtained:

{circumflex over (R)}.sub.k(1−β.sub.k){circumflex over (R)}.sub.k−1+β.sub.k({tilde over (Z)}.sub.k/k−1{tilde over (Z)}.sub.k/k−1.sup.T−H.sub.kP.sub.K/K−1H.sub.k.sup.T)

[0094] In traditional Sage-Husa filtering, it is believed that:

[00032]${\beta }_k=\frac{1-b}{1-b_k}$

[0095] where b is a fading factor. However, with increase in number k of filterings, b.sub.k may approach 0, a weight of adaptive filtering may approach 1-b and would keep invariable; and meanwhile, a weight value distributed to {circumflex over (R)}.sub.k by an initial value {circumflex over (R)}.sub.0 attenuates gradually and approaches a constant value 0 gradually. Due to the above reasons, the adaptation degree of a noise estimator is lowered, followed by weakening of the filtering accuracy.

[0096] According to a predicted residual method, whether filter divergence occurs or not may be artificially judged; if filtering is normal, general filtering is conducted; and if filter divergence is detected, optimal β.sub.k is calculated in real time, and filter divergence is prevented.

[0097] A filter divergence criterion is as follows:

{tilde over (Z)}.sub.k/k−1.sup.T{tilde over (Z)}.sub.k/k−1>γ.Math.tr[E({tilde over (Z)}.sub.k/k−1{tilde over (Z)}.sub.k/k−1.sup.T)]

[0098] and if the above equation holds, filter divergence is indicated, where γ is a reserve coefficient; and when γ=1, a convergent criterion is the most rigorous, and a following equation may be obtained by employing the most rigorous convergent criterion:

[00033]$\begin{array}{c}{\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}=\mathrm{tr}\left[E\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T\right)\right]\\ =\mathrm{tr}\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)\end{array}$

[0099] R.sub.k in the equation is substituted by {circumflex over (R)}.sub.k, and a following equation may be obtained:

[00034]$\begin{array}{c}{\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}=\mathrm{tr}\left[E\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T\right)\right]\\ =\mathrm{tr}\left(H_k{P}_{k/k-1}{H}_k^T+\left(1-{\beta }_k\right){\stackrel{⌢}{R}}_{k-1}+{\beta }_k\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{K/K-1}{H}_k^T\right)\right)\end{array}$

[0100] solve to obtain:

[00035]${\beta }_k=\frac{{\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}-tr\left(H_k{P}_{k/k-1}{H}_k^T\right)-tr\left({\stackrel{⌢}{R}}_{k-1}\right)}{\mathrm{tr}\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T\right)-tr\left({\stackrel{⌢}{R}}_{k-1}\right)}$

[0101] From the above equations:

[00036]${\beta }_k=\{\begin{array}{cc}\frac{\left({\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}-tr\left(H_k{P}_{k/k-1}{H}_k^T\right)-\mathrm{tr}\left({\stackrel{⌢}{R}}_{k-1}\right)\right)}{\left(\mathrm{tr}\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T\right)-\mathrm{tr}\left({\stackrel{⌢}{R}}_{k-1}\right)\right)}& {\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}>tr\left[E\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T\right)\right]\\ 1& {\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}\le tr\left[E\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T\right)\right]\end{array}$

[0102] Therefore, the algorithm employing improved Sage-Husa adaptive filtering in step 5 has the following process:

[00037]$X_k=X_{k,k-1}+K_k\left(Z_k-H_k{X}_{k,k-1}\right)$$K_k=P_{k,k-1}{H_k^T\left(H_k{P}_{k,k-1}{H}_k^T+R_k\right)}^{-1}$${\hat{R}}_k=\left(1-{\beta }_k\right){\stackrel{⌢}{R}}_{k-1}+{\beta }_k\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{K/K-1}{H}_k^T\right)$${\beta }_k=\{\begin{array}{cc}\frac{\left({\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}-tr\left(H_k{P}_{k/k-1}{H}_k^T\right)-\mathrm{tr}\left({\stackrel{⌢}{R}}_{k-1}\right)\right)}{\left(\mathrm{tr}\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T\right)-\mathrm{tr}\left({\stackrel{⌢}{R}}_{k-1}\right)\right)}& {\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}>tr\left[E\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T\right)\right]\\ 1& {\tilde{Z}}_{k/k-1}^T{\tilde{Z}}_{k/k-1}\le tr\left[E\left({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T\right)\right]\end{array}$$P_{k,k-1}=A_{k,k-1}{P}_{k-1}{A}_{k,k-1}^T+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^T$$P_k=\left(I-K_k{H}_k\right)P_{k,k-1}$

[0103] where X.sub.k represents a state variable of the carrier at the time k; A represents the state transition matrix of the system from the time k to a time k+1;

[0104] Z.sup.k is the observation vector of the sensor at the time k; H is a transition matrix of the system from the state space to the observation space; K.sup.k is a Kalman filtering gain at the time k; Q is a noise covariance matrix of the system; R is an observation covariance matrix; and P is an error covariance matrix.

Specific Embodiment

[0105] In order to verify the correctness of a proposed algorithm, provided is a simulation test based on a Matlab platform herein, and simulation parameters are set as follows:

[0106] 1. Settings of Inertial Element Indexes and Navigation Initial Parameters:

σ.sub.δr.sup.2(0)=(1 m).sup.2

σ.sub.δv.sup.2(0)=(0.1 m/s).sup.2

σ.sub.δα.sup.2(0)=σ.sub.δβ.sup.2(0)=(10″).sup.2σ.sub.δγ(0)=(1′).sup.2

[0107] Zero bias stability of gyroscope: eb=0.2°/h

[0108] Zero bias stability of accelerometer: db=100 ug

[0109] Angle random walk: web=0.018°/√{square root over (h)}

[0110] DVL velocity offset error: σ.sub.δv.sub.d.sup.2(0.01 m/s).sup.2

[0111] DVL drift angle error: σ.sub.δΔ.sup.2=(1′).sup.2

[0112] DVL calibration factor error: σ.sub.δC.sup.2=(0.001).sup.2

[0113] 2. Error Analysis

[0114] In combination with the above parameters, comparison is conducted by employing a standard Kalman filtering algorithm and a motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering of the present invention. FIG. 5 shows position errors and velocity errors of the integrated system with the standard Kalman filtering algorithm. It can be seen that with the standard Kalman filtering algorithm, there is lack of knowledge of the characteristic of statistical noise of the system at the beginning of experimental calculation, and a filtering error is relatively large. By using the improved Sage-Husa adaptive filtering algorithm with additional motion constraints, velocities of the carrier in three directions are constrained, adaptive filtering is conducted on measurement noise, divergence of an error of the integrated system may be well restrained, and the navigation accuracy of the system is further greatly improved, as shown in FIG. 6.

[0115] In order to verify the performance of improved adaptive filtering in the case of filter divergence, ten times the measurement noise is added at measurements in a time interval from 600s to 610s. FIG. 7 and FIG. 8 show the navigation error situations of the system before and after improvement in Sage-Husa adaptive filtering algorithm respectively. With the improved Sage-Husa adaptive filtering algorithm, filter divergence may be well restrained, improving the accuracy and the robustness of the navigation system.

[0116] The technical means disclosed by the technical solution of the present invention are not limited to these disclosed in the above implementations and further include the technical solution formed by any combination of the above technical features. It should be noted that several improvements and embellishments may also be made without departing from the principles of the present invention to those of ordinary skill in the art, and these improvements and embellishments should also be considered to fall within the scope of the present invention.