Robust Filtering Method for Integrated Navigation Based on Statistical Similarity Measure

Abstract

The disclosure belongs to the technical field of integrated navigation under non-ideal conditions, and in particular relates to a robust filtering method for integrated navigation based on a statistical similarity measure (SSM). In view of the situation that there are normal beam measurement information of the DVL and beam measurement information with a large error simultaneously in a SINS/DVL tightly integrated system, and aiming at the problem that the existing robust filters of an integrated navigation system process the measurement information in a rough manner and are likely to lead to loss of normal measurement information, the disclosure proposes a novel robust filtering method based on decomposition of multi-dimensional measurement equations and the SSM. The disclosure introduces the SSM theory while decomposing the multi-dimensional measurement equations of the SINS/DVL tightly integrated navigation system, and assists the measurement noise variance of each beam to complete respective adaptive update in case of a large measurement error, finally ensuring independence of processing of the measurement information of each beam. The disclosure can be used in the field of integrated navigation of underwater vehicles under non-ideal conditions.

Claims

1. A robust filtering method for integrated navigation based on a statistical similarity measure, comprising the following steps: step 1: defining coordinate systems, comprising the following steps: defining that a body coordinate system is represented by b, an “earth-centered, earth-fixed” coordinate system is represented by e, an “east-north-up” geographic coordinate system is represented by n as a navigation system, an inertial coordinate system is represented by i, and a DVL coordinate system is represented by d; defining velocities in different coordinate systems:
V.sub.SINS.sup.b=[V.sub.x.sup.b V.sub.y.sup.b V.sub.z.sup.b].sup.T
V.sub.SINS.sup.n=[V.sub.SINS_E.sup.n V.sub.SINS_N.sup.n V.sub.SINS_U.sup.n].sup.T
V.sub.SINS.sup.d=[V.sub.SINS_1.sup.d V.sub.SINS_2.sup.d V.sub.SINS_3.sup.d V.sub.SINS_4.sup.d].sup.T
V.sub.DVL.sup.d=[V.sub.DVL_1.sup.d V.sub.DVL_2.sup.d V.sub.DVL_3.sup.d V.sub.DVL_4.sup.d].sup.T wherein V.sub.SINS.sup.b represents a velocity vector of a strap-down inertial navigation system in the b-frame; V.sub.SINS.sup.n represents a velocity vector of the strap-down inertial navigation system in the n-frame; V.sub.SINS.sup.d represents a velocity vector of the strap-down inertial navigation system in the d-frame; and V.sub.DVL.sup.d represents a velocity vector of a beam of the DVL in the d-frame; step 2: establishing state equations of a SINS/DVL tightly integrated navigation system: comprising the following steps: selecting errors of the SINS as the states, comprising a misalignment angle ϕ, a velocity error δV.sup.n , a position error δp, a gyro constant drift ε.sup.b and an accelerometer constant zero bias ∇.sup.b , and establishing state equations as follows: $$\begin{gathered} X={\left[\delta p^T\delta V^{nT}{\varphi }^T{\epsilon }^{bT}{\nabla }^{bT}\right]}^T \\ \stackrel{.}{X}=\mathrm{FX}+\mathrm{Gw} \\ w={\left[w^{b_g}{w}^{b_a}\right]}^T \\ {\mathrm{\delta \omega }}_{\mathrm{ib}}^b={\epsilon }^b+w^{b_g} \\ \delta f^b={\nabla }^b+w^{b_a} \\ \stackrel{.}{\varphi }=-{\omega }_{in}^n\times \varphi +{\mathrm{\delta \omega }}_{in}^n-C_b^n{\mathrm{\delta \omega }}_{\mathrm{ib}}^b \\ \delta {\stackrel{.}{v}}^n=f^b\times \varphi +v^n\times \left({\mathrm{\delta \omega }}_{\mathrm{ie}}^b+{\mathrm{\delta \omega }}_{in}^n\right)-\left({\mathrm{\delta \omega }}_{\mathrm{ie}}^b+{\mathrm{\delta \omega }}_{in}^n\right)\times \delta v^n+C_b^n\delta f^b+\delta g^n \\ \delta \stackrel{.}{p}=M_{\mathrm{pv}}\delta v^n+M_{pp}\delta p \\ {M}_{pv}=\left[\begin{array}{ccc}0& \frac{1}{R_M}& 0\\ \frac{\sec L}{R_N}& 0& 0\\ 0& 0& 1\end{array}\right] \\ {M}_{pp}=\left[\begin{array}{ccc}0& 0& -\frac{v_N}{{\left(R_M\right)}^2}\\ \frac{v_E\sec L\tan L}{R_N}& 0& -\frac{v_E\sec L}{{\left(R_N\right)}^2}\\ 0& 0& 0\end{array}\right] \end{gathered}$$ wherein ε.sup.b represents a gyro constant drift vector; w.sup.b.sup.g is a gyro random drift vector; ε.sup.b is an accelerometer constant zero bias vector; w.sup.b.sup.a is an accelerometer random zero bias vector; ϕ represents a misalignment angle vector consisting of pitch, roll and yaw misalignment angles; C.sub.b.sup.n is a direction cosine matrix, representing a transformation from the b-frame to the n-frame; ω.sub.in.sup.n represents an angular velocity vector in the b-frame relative to the i-frame under the b-frame, and a corresponding calculation error is δω.sub.in.sup.n=δω.sub.ie.sup.n+δω.sub.en.sup.n; v.sup.n=[v.sub.E v.sub.N v.sub.h].sup.T is a velocity vector of a body relative to the ground in the n-frame, and a corresponding error vector is δv.sup.n; ƒ.sup.b is an output of an accelerometer; δg.sup.n is a projection of a gravity error vector in the n-frame; δp represents a position error vector consisting of a latitude error δL, a longitude error δλ and an altitude error δh; and R.sub.M and R.sub.N respectively represent the radius of curvature of a meridian circle and a prime vertical circle; step 3: establishing and decomposing measurement equations of the SINS/DVL tightly integrated navigation system, comprising the following steps: measurement equations: $$\begin{gathered} Z=\left[{\hat{V}}_{\mathrm{SINS}}^d-{\hat{V}}_{DVL}^d\right] \\ =\left[C_b^d{C}_n^b\left(I_{3\times 3}+\varphi \times \right)\left(V_{\mathrm{INS}}^n+\delta V^n\right)-\left(V_{DVL}^d+\delta V_{DVL}^d\right)\right] \\ =\left[C_b^d{C}_n^b\delta V^n-C_b^d{C}_n^b{V}_{\mathrm{INS}}^n\times \varphi -\delta V_{DVL}^d\right] \\ Z=HX+v \\ H=\left[0_{4\times 3}{C}_b^d{C}_n^b-C_b^d{C}_n^b{V}_{\mathrm{SINS}}^n\times 0_{4\times 3}{0}_{4\times 3}\right] \end{gathered}$$ wherein v=−δV.sub.DVL.sup.d represents a measurement noise; I.sub.3×3 is a three-dimensional identity matrix; [⋅×]represents a cross product of vectors; V.sub.DVL.sup.d=V.sub.SINS.sup.d=C.sub.b.sup.dC.sub.n.sup.bV.sub.SINS.sup.n; C.sub.b.sup.d=[b.sub.1 b.sub.2 b.sub.3 b.sub.4 ].sup.T ; $b_i=\left[\begin{array}{c}\sin {\phi }_i\cos \alpha \\ \cos {\phi }_i\cos \alpha \\ -\sin \alpha \end{array}\right];{\phi }_i=\left(i-1\right)90°+{\phi }_0,i=1,2,3,4;$ α is a horizontal angle between the beam and the carrier; and φ.sub.0 is set as 0° or 45°; wherein measurement noises of various beams of the DVL are usually uncorrelated, and a measurement noise covariance matrix R=E[vv.sup.T] is a diagonal matrix, so the measurement equation is equivalently decomposed: $\{\begin{array}{c}{Z}^1={\hat{V}}_{\mathrm{INS}\_1}^d-{\hat{V}}_{\mathrm{DVL}\_1}^d=H^{1}X+v^1\\ Z^2={\hat{V}}_{\mathrm{INS}\_2}^d-{\hat{V}}_{\mathrm{DVL}\_2}^d=H^{2}X+v^2\\ Z^3={\hat{V}}_{\mathrm{INS}\_3}^d-{\hat{V}}_{\mathrm{DVL}\_3}^d=H^{3}X+v^3\\ Z^4={\hat{V}}_{\mathrm{INS}\_4}^d-{\hat{V}}_{\mathrm{DVL}\_4}^d=H^{4}X+v^4\\ Z^5={\hat{h}}_{PS}-h_{\mathrm{INS}}=H^{5}X+v^5\end{array}$ wherein H.sup.j represents the j-th row of a measurement matrix; v.sup.j is the corresponding measurement noise, and the measurement noises of different beams of the DVL are uncorrelated; here, the measured values of four beams of the DVL are assumed to be measured respectively by different sensors; step 4: establishing a cost function of state estimation based on a statistical similarity measure: $\left({\hat{X}}_{k+1/k+1},{\hat{P}}_{k+1/k+1}\right)=\underset{\left(X_{k+1},P_{k+1}\right)}{\arg \max }\left\{f_x\left(\mathrm{tr}\left(A_{k+1}{\hat{P}}_{k+1/k}^{-1}\right)\right)+\underset{i=1}{\overset{q}{.Math.}}{f}_z\left(\mathrm{tr}\left({B_{k+1}^j\left(R_{k+1}^j\right)}^{-1}\right)\right)\right\}=\underset{\left(X_{k+1},P_{k+1}\right)}{\arg \max }J\left(X_{k+1},P_{k+1}\right)A_{k+1}=\int \left(X-{\hat{X}}_{k+1/k}\right){{\left(X-{\hat{X}}_{k+1/k}\right)}^{}}^{T}N\left(X;X_{k+1},P_{k+1}\right)\mathrm{dX}=\left(X-{\hat{X}}_{k+1/k}\right){{\left(X-{\hat{X}}_{k+1/k}\right)}^{}}^T+P_{k+1}{B}_{k+1}^j=\int \left(Z_{k+1}^j-h_{k+1}^j\left(X\right)\right){\left(Z_{k+1}^j-h_{k+1}^j\left(X\right)\right)}^{T}N\left(X;X_{k+1},P_{k+1}\right)\mathrm{dX}$ wherein {circumflex over (X)}.sub.k+1/k and {circumflex over (P)}.sub.k+1/k respectively represent a state one-step prediction result and a one-step prediction covariance matrix; N(X;X.sub.k+1, P.sub.k+1) represents a Gaussian distribution about X, a mean is X.sub.k+1, a variance is P.sub.k+1; a similarity function ƒ.sub.x(t) is selected as ƒ.sub.x(t)=−0.5t; a similarity function ƒ.sub.z(t) is selected as ƒ.sub.z(t)=−√{square root over ((ω+m)(ω+t))}, ωis the dof parameter, ωis the dimension of measurement, m=1; and q=4; step 5: applying Gauss-Newton iteration to find an optimal value of the cost function, comprising the following steps: step 5.1: initializing the number of iterations t=0, setting an initial value of iteration as {circumflex over (X)}.sub.k+1/k+1.sup.0={circumflex over (X)}.sub.k+1/k, and {circumflex over (P)}.sub.k+1/k+1.sup.0={circumflex over (P)}.sub.k+1/k; step 5.2: calculating a filter gain K.sub.k+1.sup.j(t); $K_{k+1}^{j\left(t\right)}={\left({\xi }_{k+1}^t{\hat{P}}_{k+1/k}^{-1}+\underset{i=1}{\overset{q}{.Math.}}{{\gamma }_{k+1}^{i\left(t\right)}\left(H_{k+1}\right)}^T{\left(R_{k+1}^i\right)}^{-1}{H}_{k+1}^i\right)}^{-1}{{\gamma }_{k+1}^{j\left(t\right)}\left(H_{k+1}^j\right)}^T{\left(R_{k+1}^j\right)}^{-1}$ wherein $H_{k+1}^j=\frac{\partial H_{k+1}^j\left(X_{k+1}\right)}{\partial X_{k+1}}{.Math.}_{X_{k+1}={\hat{X}}_{k+1/k+1}^t};$ ξ.sub.k+1.sup.t and γ.sub.k+1.sup.i(t) are equivalent to auxiliary parameters used for adjusting the one-step prediction covariance matrix and a measurement noise variance; step 5.3: calculating a posterior state {circumflex over (X)}.sub.k+1/k+1.sup.t+1 ${\hat{X}}_{k+1/k+1}^{t+1}={\hat{X}}_{k+1/k}+\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}\left(Z_{k+1}^j-h_{k+1}^j\left({\hat{X}}_{k+1/k+1}^t\right)-H_{k+1}^j\left({\hat{X}}_{k+1/k}^t-{\hat{X}}_{k+1/k+1}^t\right)\right)$ step 5.4: calculating an estimated error covariance matrix {circumflex over (P)}.sub.k+1/k+1.sup.t: ${\hat{P}}_{k+1/k+1}^t=\left(I-\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}{H}_{k+1}^j\right){{\hat{P}}_{k+1/k}\left(I-\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}{H}_{k+1}^j\right)}^T+\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}{R_{k+1}^j\left(K_{k+1}^{j\left(t\right)}\right)}^T;$ and step 5.5: judging whether $\frac{\partial J\left(X_{k+1},P_{k+1}\right)}{\partial X_{k+1}}{|}_{X_{k+1}={\hat{X}}_{k+1/k+1}^t,P_{k+1}={\hat{P}}_{k+1/k+1}^t}=0$ is met; if not, setting t=t+1, returning to step 5.2; otherwise, setting X.sub.k+1={circumflex over (X)}.sub.k+1/k+1.sup.t, P.sub.k+1={circumflex over (P)}.sub.k+1/k+1.sup.t, and outputting an error X of the SINS; and step 6: feeding back the estimated error X of the SINS to the SINS for correction, comprising the following steps: according to the estimated error X of the SINS, obtaining the misalignment angle ϕ, the velocity error δV.sup.nL , and the position error δp, wherein for an attitude matrix C.sub.b.sup.n′, a velocity {circumflex over (V)}.sup.n, and a position {circumflex over (p)} solved by the SINS, a feedback correction method of attitude errors is:
C.sub.b.sup.n=(I+ϕx)C.sub.b.sup.n′; and subtracting the velocity error δV.sup.n and the position error δp directly from the output of the SINS:
V.sup.n={circumflex over (V)}.sup.n−δV.sup.n,p={circumflex over (p)}−δp.

Description

BRIEF DESCRIPTION OF FIGURES

[0032] FIG. 1 is a structure diagram of the SINS/DVL tightly integrated navigation system.

[0033] FIG. 2 is a diagram showing a velocity relationship in different coordinate systems.

[0034] FIG. 3 a diagram showing the experimental trajectory of a lake trial.

[0035] FIG. 4 is a comparison diagram between velocity errors and positioning errors.

[0036] FIG. 5 is a comparison diagram between beam measured values of the DVL and change of auxiliary parameters.

[0037] FIG. 6 is a flow chart of implementation of the disclosure.

DETAILED DESCRIPTION

[0038] The disclosure is further described below with reference to the accompanying drawings.

[0039] The objective of the disclosure is to provide a robust filtering method for individually adjusting the measurement noise variance of each beam of the DVL for a SINS/DVL tightly integrated navigation system. That is, when a certain measurement has a large error, the corresponding measurement noise variance matrix in a filter can be automatically increased, while the measurement noise variance matrices of other normal measurements are hardly adjusted.

[0040] Step 1: Establishment of a SINS/DVL tightly integrated navigation system model

[0041] First, common coordinate systems are defined. A body coordinate system is represented by b. An “Earth-centered, Earth-fixed” coordinate system is represented by e. An “East-North-Up” geographic coordinate system is represented by n as a navigation system. An inertial coordinate system is represented by i. A DVL coordinate system is represented by d.

[0042] FIG. 1 depicts the structure of the SINS/DVL tightly integrated navigation system. The SINS, as a main navigation device, provides velocity, attitude and position information. The DVL, as a navigation aid, provides four-dimensional measurement vectors, that is, the velocities of the DVL relative to the seabed in four beam directions. A SINS solution is transformed into a difference between the velocity in the d-frame and the velocity of a beam of the DVL as the observation of a navigation filter. Then the state is estimated and fed back to the SINS to reduce the navigation error of the SINS.

[0043] In the disclosure, it is assumed that an installing angle error between the SINS and the DVL and the scale factor of the DVL have been calibrated and compensated after the device is installed. Residual installing angle errors and scale factor errors are negligible. Therefore, the disclosure only selects the errors of the SINS as the states, including a misalignment angle ϕ , a velocity error δV.sup.n, a position error δp, a gyro constant drift ε.sup.b and an accelerometer constant zero bias ∇.sup.b. The state equations are as follows:

[00011]$\begin{array}{cc}X={\left[\begin{array}{ccccc}\delta p^T& \delta V^{nT}& {\varphi }^T& {\epsilon }^{bT}& V^{bT}\end{array}\right]}^T& \left(1\right)\end{array}$$\begin{array}{cc}\stackrel{.}{X}=\mathrm{FX}+\mathrm{Gw}& \left(2\right)\end{array}$$\begin{array}{cc}w={\left[\begin{array}{cc}{w}^{b_g}& w^{b_a}\end{array}\right]}^T& \left(3\right)\end{array}$$\begin{array}{cc}{\mathrm{\delta \omega }}_{\mathrm{ib}}^b={\epsilon }^b+w^{b_g}& \left(4\right)\end{array}$$\begin{array}{cc}\delta f^b={\nabla }^b+w^{b_a}& \left(5\right)\end{array}$$\begin{array}{cc}\stackrel{.}{\varphi }=-{\omega }_{\mathrm{in}}^n\times \varphi +{\mathrm{\delta \omega }}_{\mathrm{in}}^n-C_b^n{\mathrm{\delta \omega }}_{\mathrm{ib}}^b& \left(6\right)\end{array}$$\begin{array}{cc}\delta {\stackrel{.}{v}}^n=f^b\times \varphi +v^n\times \left({\mathrm{\delta \omega }}_{\mathrm{ie}}^n+{\mathrm{\delta \omega }}_{\mathrm{in}}^n\right)-\left({\mathrm{\delta \omega }}_{\mathrm{ie}}^n+{\mathrm{\delta \omega }}_{\mathrm{in}}^n\right)\times \delta v^n+C_b^n\delta f^b+\delta g^n& \left(7\right)\end{array}$$\begin{array}{cc}\delta \stackrel{.}{p}=M_{\mathrm{pv}}\delta v^n+M_{\mathrm{pp}}\delta p& \left(8\right)\end{array}$$\begin{array}{cc}{M}_{pv}=\left[\begin{array}{ccc}0& \frac{1}{R_M}& 0\\ \frac{\sec L}{R_N}& 0& 0\\ 0& 0& 1\end{array}\right]& \left(9\right)\end{array}$$\begin{array}{cc}{M}_{\mathrm{pp}}=\left[\begin{array}{ccc}0& 0& -\frac{v_N}{{\left(R_M\right)}^2}\\ \frac{v_E\sec L\tan L}{R_N}& 0& -\frac{v_E\sec L}{{\left(R_N\right)}^2}\\ 0& 0& 0\end{array}\right]& \left(10\right)\end{array}$

[0044] where ε.sup.b represents a gyro constant drift vector, and w.sup.b.sup.g is a gyro random drift vector. ∇.sup.b is an accelerometer constant zero bias vector. w.sup.b.sup.a is an accelerometer random zero bias vector. ϕ represents a misalignment angle vector consisting of pitch, roll and yaw misalignment angles. C.sub.b.sup.n is a direction cosine matrix, representing a transformation from the b-frame to the n-frame. ω.sub.in.sup.n represents an angular velocity vector in the b-frame relative to the iframe under the b-frame, and a corresponding calculation error is δω.sub.in.sup.n=δω.sub.ie.sup.n+δω.sub.en .sup.n. v.sup.n=[v.sub.Ev.sub.Nv.sub.h].sup.T is a velocity vector of a body relative to the ground in the n-frame, and the corresponding error vector is δv.sup.n . ƒ.sup.b is an output of an accelerometer. δg.sup.n is a projection of a gravity error vector in the n-frame. δp represents a position error vector consisting of a latitude error δL , a longitude error δλ and an altitude error δh. R.sub.M and R.sub.N respectively represent the radius of curvature of a meridian circle and a prime vertical circle.

[0045] Before introducing measurement equations, the disclosure defines the velocities in different coordinate systems, taking a four-beam configuration DVL as an example, as shown in FIG. 2. V.sub.SINS.sup.b is a velocity vector of a strap-down inertial navigation system in the b-frame. V.sub.SINS.sup.n represents a velocity vector of the strap-down inertial navigation system in the n-frame. V.sub.SINS.sup.d represents a velocity vector of the strap-down inertial navigation system in the d-frame. V.sub.DVL.sup.d represents a velocity vector of a beam of the DVL in the d-frame.

V.sub.SINS.sup.b=[V.sub.x.sup.b V.sub.y.sup.b V.sub.z.sup.b].sup.T   (11)

V.sub.SINS.sup.n=[V.sub.SINS_E.sup.n V.sub.SINS_N.sup.n V.sub.SINS_U.sup.n].sup.T   (12)

V.sub.SINS.sup.d=[V.sub.SINS_1.sup.d V.sub.SINS_2.sup.d V.sub.SINS_3.sup.d V.sub.SINS_4.sup.d].sup.T   (13)

V.sub.DVL.sup.d=[V.sub.DVL_1.sup.d V.sub.DVL_2.sup.d V.sub.DVL_3.sup.d V.sub.DVL_4.sup.d].sup.T   (14)

[0046] The relationship between the velocity of the strap-down inertial navigation system in the navigation frame and the beam velocity of the DVL in the d-frame is as follows:

[00012]$\begin{array}{cc}{V}_{\mathrm{DVL}}^d=V_{\mathrm{SINS}}^d=C_b^d{C}_n^b{V}_{\mathrm{SINS}}^n& \left(15\right)\end{array}$$\begin{array}{cc}{C}_b^d={\left[\begin{array}{cccc}{b}_1& b_2& b_3& b_4\end{array}\right]}^T& \left(16\right)\end{array}$$\begin{array}{cc}{b}_i=\left[\begin{array}{c}\sin {\phi }_i\cos \alpha \\ \cos {\phi }_i\cos \alpha \\ =\sin \alpha \end{array}\right]& \left(17\right)\end{array}$$\begin{array}{cc}{\phi }_i=\left(i-1\right)90°+{\phi }_0,i=1,2,3,4& \left(18\right)\end{array}$

[0047] where α is a horizontal angle between the beam and the carrier. Generally, φ.sub.0 is set as 0° or 45° .

[0048] The measured value of a beam of the DVL is modeled as follows:

{circumflex over (V)}.sub.DVL.sup.d=V.sub.DVL.sup.d+δV.sub.DVL.sup.d   (19)

[0049] where δV.sub.DVL.sup.d represents a measurement noise of the DVL. The result of converting the velocity output of the strap-down inertial navigation system to the d-frame is:

{circumflex over (V)}.sub.SINS.sup.d=C.sub.b.sup.dC.sub.n.sup.bC.sub.n′.sup.n{circumflex over (V)}.sub.SINS.sup.n=C.sub.b.sup.dC.sub.n.sup.b(I.sub.3×3+ϕx)(V.sub.SINS.sup.n+δV.sup.n)   (20)

[0050] where I.sub.3×3 is a three-dimensional identity matrix. [⋅×] represents a cross product of vectors.

[0051] Simultaneous equations (19) and (20) can obtain measurement equations:

[00013]$\begin{array}{cc}Z=\left[{\widehat{\text{⁠}V}}_{\mathrm{SINS}}^d-{\hat{V}}_{\mathrm{DVL}}^d\right]=\left[C_b^d{C}_n^b\left(I_{3\times 3}+\varphi \times \right)\left(V_{\mathrm{INS}}^n+\delta V^n\right)-\left(V_{\mathrm{DVL}}^d+\delta V_{\mathrm{DVL}}^d\right)\right]=\left[C_b^d{C}_n^b\delta V^n-C_b^d{C}_n^b{V}_{\mathrm{INS}}^n\times \varphi -\delta V_{\mathrm{DVL}}^d\right]& \left(21\right)\end{array}$$\begin{array}{cc}Z=\mathrm{HX}+v& \left(22\right)\end{array}$$\begin{array}{cc}H=\left[\begin{array}{ccccc}{0}_{4\times 3}& C_b^d{C}_n^b& -C_b^d{C}_n^b{V}_{\mathrm{SINS}}^n\times & 0_{4\times 3}& 0_{4\times 3}\end{array}\right]& \left(23\right)\end{array}$

[0052] where v=−δV.sub.DVL.sup.d represents the measurement noise.

[0053] Step 2: Decomposition of multi-dimensional measurement equations of the SINS/DVL tightly integrated navigation system

[0054] The measurement noises of beams of the DVL are usually uncorrelated, and a measurement noise covariance matrix R=E[vv.sup.T] is a diagonal matrix, so in the disclosure, the measurement equation (22) can be equivalently decomposed:

[00014]$\begin{array}{cc}\{\begin{array}{c}{Z}^1={\hat{V}}_{\mathrm{INS}\_1}^d-{\hat{V}}_{\mathrm{DVL}\_1}^d=H^{1}X+v^1\\ Z^2={\hat{V}}_{\mathrm{INS}\_2}^d-{\hat{V}}_{\mathrm{DVL}\_2}^d=H^{2}X+v^2\\ Z^3={\hat{V}}_{\mathrm{INS}\_3}^d-{\hat{V}}_{\mathrm{DVL}\_3}^d=H^{3}X+v^3\\ Z^4={\hat{V}}_{\mathrm{INS}\_4}^d-{\hat{V}}_{\mathrm{DVL}\_4}^d=H^{4}X+v^4\\ Z^5={\hat{h}}_{PS}-h_{\mathrm{INS}}=H^{5}X+v^5\end{array}& \left(24\right)\end{array}$

[0055] where H.sup.j represents the j-th row of a measurement matrix. v.sup.j is the corresponding measurement noise, and the measurement noises of different beams of the DVL are uncorrelated. Here, the measured values of four beams of the DVL are assumed to be measured respectively by different sensors.

[0056] Step 3: Introduction of the concept of statistical similarity measure, and establishment of a cost function of state estimation based on the statistical similarity measure

[0057] Step 3.1: Introduction of the statistical similarity measure (SSM)

[0058] A statistical similarity measure (SSM).sup.s(a,b) may be used to represent the similarity between two random vectors a and b, defined as follows:

s(a,b)=E[ƒ(∥a −b∥.sup.2)]=∫∫ƒ(∥a−b∥.sup.2)p(a,b)dadb tm (25)

[0059] where ∥⋅∥ represents a Euclidean norm, p(a,b) represents a joint probability density between the random vectors a and b. ƒ(⋅) represents a similarity function and satisfies the following three conditions. [0060] a) ƒ(⋅) is a continuous function in a domain of definition [0, +∞). [0061] b) ƒ(⋅) is a monotonically decreasing function, i.e. {dot over (ƒ)}(⋅)<0. [0062] c) The second derivative of ƒ(⋅) is non-negative, i.e. {umlaut over (ƒ)}(⋅)≥0.

[0063] A statistical similarity measure conforms to a usual definition of a similarity measure. The higher the similarity between random vectors, the larger the statistical similarity measure. When ƒ(t) is selected as ƒ(t)=−t, the SSM represents a negative mean squared error between

[0064] different random vectors. When ƒ(t) is selected as

[00015]$f\left(t\right)=\exp \left(-\frac{t}{2{\sigma }^2}\right),$

the SSM represents a correlation entropy between different random vectors. By selecting different similarity functions, different SSMs can be implemented.

[0065] Step 3.2: Establishment of a robust cost function

[0066] First, a system model is assumed as

X.sub.k+1=ƒ.sub.k(X.sub.k)+n.sub.k   (26)

Z.sub.k+1.sup.j=h.sub.k+1.sup.j(X.sub.k+1)+v.sub.k+1.sup.j,j=1,2,. . .,q   (27)

[0067] where k represents time, X.sub.k+1 represents a p-dimensional state. Z.sub.k+1.sup.j represents the j-th measurement. ƒ.sub.k(⋅) and h.sub.k+1.sup.j (⋅) represent a state transition function and a measurement function. n.sub.k and v.sub.k+1.sup.j represent a process noise and a measurement noise. Q.sub.k and R.sub.k.sup.j represent a nominal covariance matrix of the process noise and a nominal covariance matrix of the j-th measurement noise. For the SINS/DVL tight integrated navigation system introduced in the disclosure, q is 4.

[0068] The standard Kalman filter (KF) is essentially a weighting of state prediction and measurement information to obtain an optimal estimate of the state, and its weighted least squares cost function reflects the mean squared error between the state and the predicted state, and between the measurement and the predicted measurement. The disclosure establishes a cost function for reflecting the SSM between the state and the predicted state, and between the measurement and the predicted measurement.

[00016]$\begin{array}{cc}Q\left(X_{k+1}\right)=\underset{q\left(X_{k+1}\right)}{\mathrm{argmax}}\left\{s\left(S_{{\hat{P}}_{k+1/k}}^{-1}{X}_{k+1},S_{{\hat{P}}_{k+1/k}}^{-1}{\hat{X}}_{k+1/k}\right)+\underset{j=1}{\overset{q}{.Math.}}s\left(S_{R_{k+1}^j}^{-1}{Z}_{k+1}^j,S_{R_{k+1}^j}^{-1}{h}_{k+1}^j\left(X_{k+1}\right)\right)\right\}=\underset{q\left(X_{k+1}\right)}{\mathrm{argmax}}\left\{\int f_x\left({.Math.S_{{\hat{P}}_{k+1/k}}^{-1}\left(X_{k+1}-{\hat{X}}_{k+1/k}\right).Math.}^2\right)q\left(X_{k+1}\right){\mathrm{dX}}_{k+1}+\underset{j=1}{\overset{q}{.Math.}}\int f_z\left({.Math.S_{R_{k+1}^j}^{-1}\left(Z_{k+1}^j-h_{k+1}^j\left(X_{k+1}\right)\right).Math.}^2\right)q\left(X_{k+1}\right){\mathrm{dX}}_{k+1}\right\}& \left(28\right)\end{array}$

[0069] where {circumflex over (X)}.sub.k+1/k and {circumflex over (P)}.sub.k+1/k represent a state one-step prediction result and a one-step prediction covariance matrix. S.sub.R.sub.k+1.sub.j and S.sub.{circumflex over (P)}.sub.k+1/k represent root mean square decomposition of the measurement noise variance R.sub.k.sup.j and the one-step prediction covariance matrix {circumflex over (P)}.sub.k+1/k.

[0070] For solving the cost function shown in (28), a posterior distribution is approximated as a Gaussian distribution and a lower bound of the cost function in the above equation is solved by using Jensen inequality, which is used as a new cost function.

[00017]$\begin{array}{cc}\left({\hat{X}}_{k+1/k+1},{\hat{P}}_{k+1/k+1}\right)=\underset{\left(X_{k+1},P_{k+1}\right)}{\mathrm{argmax}}\left\{f_x\left(\mathrm{tr}\left(A_{k+1}{\hat{P}}_{k+1/k}^{-1}\right)\right)+\underset{j=1}{\overset{q}{.Math.}}{f}_z\left(\mathrm{tr}\left({B_{k+1}^j\left(R_{k+1}^j\right)}^{-1}\right)\right)\right\}\underset{\left(X_{k+1},P_{k+1}\right)}{\mathrm{argmax}}J\left(X_{k+1},P_{k+1}\right)& \left(29\right)\end{array}$$\begin{array}{cc}{A}_{k+1}=\int \left(X-{\hat{X}}_{k+1/k}\right){\left(X-{\hat{X}}_{k+1/k}\right)}^{T}N\left(X;X_{k+1},P_{k+1}\right)\mathrm{dX}=\left(X_{k+1}-{\hat{X}}_{k+1/k}\right){\left(X_{k+1}-{\hat{X}}_{k+1/k}\right)}^T+P_{k+1}& \left(30\right)\end{array}$$\begin{array}{cc}{B}_{k+1}^j=\int \left(Z_{k+1}^j-h_{k+1}^j\left(X\right)\right){\left(Z_{k+1}^j-h_{k+1}^j\left(X\right)\right)}^{T}N\left(X;X_{k+1},P_{k+1}\right)\mathrm{dX}& \left(31\right)\end{array}$

[0071] where N(X;X.sub.k+1,P.sub.k+1) represents a Gaussian distribution about X, a mean is X.sub.k+1, and a variance is P.sub.k+1.

[0072] When the measurement equation h.sub.k+1.sup.j(⋅) is nonlinear, Sigma point transformation, such as an unscented transformation rule or a third-degree spherical-radial cubature rule, may be used to approximate the solution. When the measurement equation is linear, (31) may be written as

B.sub.k+1.sup.j=(Z.sub.k+1.sup.j−H.sub.k+1.sup.jX.sub.k+1)(Z.sub.k+1.sup.j−H.sub.k+1.sup.jX.sub.k+1).sup.T+H.sub.k+1.sup.jP.sub.k+1(H.sub.k+1.sup.j).sup.T   (32).

[0073] Step 4: Application of Gauss-Newton iteration to find the optimal value of the cost function

[0074] To solve the extremum of the cost function, in general, the method of taking the derivative of the cost function and setting the derivative to zero can be used for calculation.

[00018]$\begin{array}{cc}{\frac{\partial J\left(X_{k+1},P_{k+1}\right)}{\partial X_{k+1}}.Math.}_{X_{k+1}={\hat{X}}_{k+1/k+1}^t,P_{k+1}={\hat{P}}_{k+1/k+1}}=0& \left(33\right)\end{array}$

[0075] Since equation (33) is a nonlinear equation, (33) is solved by the Gauss-Newton iteration method in the disclosure.

[0076] According to the Gauss-Newton iteration method, for the nonlinear equation (33), the numerical update process of the solution is:

[00019]$\begin{array}{cc}{{\hat{X}}_{k+1/k+1}^{t+1}={\hat{X}}_{k+1/k+1}^t-{\tilde{\Theta }}^{-1}\left({\hat{X}}_{k+1/k+1}^t,{\hat{P}}_{k+1/k+1}^t\right)\frac{\partial J\left(X_{k+1},P_{k+1}\right)}{\partial X_{k+1}}.Math.}_{X_{k+1}={\hat{X}}_{k+1/k+1}^t,P_{k+1}={\hat{P}}_{k+1/k+1}^t}& \left(34\right)\end{array}$

[0077] where the superscript t denotes the t-th iteration. An initial value of iteration is set to {circumflex over (X)}.sub.k+1/k+1.sup.0={circumflex over (X)}.sub.k+1/k, {circumflex over (P)}.sub.k+1/k+1.sup.0={circumflex over (P)}.sub.k+1/k. {tilde over (Θ)}({circumflex over (X)}.sub.k+1/k+1.sup.t,{circumflex over (P)}.sub.k+1/k+1.sup.1) represents an approximate Hessian matrix of the cost function J(X.sub.k+1,P.sub.k+1).

[00020]$\begin{array}{cc}{\frac{\partial J\left(X_{k+1},P_{k+1}\right)}{\partial X_{k+1}}.Math.}_{X_{k+1}={\hat{X}}_{k+1/k+1},P_{k+1}={\hat{P}}_{k+1/k+1}}=-{\xi }_{k+1}^t{\hat{P}}_{k+1/k}^{-1}\left({\hat{X}}_{k+1/k+1}^t,{\hat{X}}_{k+1/k}\right)+\underset{j=1}{\overset{q}{.Math.}}{{\gamma }_{k+1}^{j\left(t\right)}\left(H_{k+1}^j\right)}^T{\left(R_{k+1}^j\right)}^{-1}\left(Z_{k+1}^j-h_{k+1}^j\left({\hat{X}}_{k+1/k+1}^t\right)\right)& \left(35\right)\end{array}$$\begin{array}{cc}\tilde{\Theta }\left({\hat{X}}_{k+1/k+1}^t,{\hat{P}}_{k+1/k+1}^t\right)=-{\xi }_{k+1}^t{\hat{P}}_{k+1/k}^{-1}-\underset{j=1}{\overset{q}{.Math.}}{{\gamma }_{k+1}^{j\left(t\right)}\left(H_{k+1}^j\right)}^T{\left(R_{k+1}^j\right)}^{-1}{H}_{k+1}^j& \left(36\right)\end{array}$

[0078] where

[00021]$\begin{array}{cc}{H}_{k+1}^j=\frac{\partial h_{k+1}^j\left(X_{k+1}\right)}{\partial X_{k+1}}{.Math.}_{X_{k+1}={\hat{X}}_{k+1/k+1}^t}\{\begin{array}{c}{\xi }_{k+1}^t=-2{\stackrel{.}{f}}_x\left(\mathrm{tr}\left(A_{k+1}^{*}{\hat{P}}_{k+1/k}^1\right)\right)\\ {\gamma }_{k+1}^{j\left(t\right)}=-2{\stackrel{.}{f}}_z\left(\mathrm{tr}\left({B_{k+1}^{j\text{(*})}\left(R_{k+1}^j\right)}^{-1}\right)\right)\end{array}& \left(37\right)\end{array}$$\begin{array}{cc}{A}_{k+1}^{*}={\hat{P}}_{k+1/k+1}^i+\left({\hat{X}}_{k+1/k+1}^{t+1}-{\hat{X}}_{k+1/k}\right){\left({\hat{X}}_{k+1/k+1}^{t+1}-{\hat{X}}_{k+1/k}\right)}^T& \left(38\right)\end{array}$$\begin{array}{cc}{B}_{k+1}^{j\text{(*})}=\int \left(Z_{k+1}^j-h_{k+1}^j\left(X\right)\right){\left(Z_{k+1}^j-h_{k+1}^j\left(X\right)\right)}^{T}N\left(X;{\hat{X}}_{k+1/k+1}^{t+1},{\hat{P}}_{k+1/k+1}^t\right)\mathrm{dX}& \left(39\right)\end{array}$

[0079] Sorted out:

[00022]$\begin{array}{cc}{\hat{X}}_{k+1/k+1}^{t+1}={\hat{X}}_{k+1/k}+{\left({\xi }_{k+1}^t{\hat{P}}_{k+1/k}^1+\underset{j=1}{\overset{q}{.Math.}}{{\gamma }_{k+1}^{j\left(t\right)}\left(H_{k+1}^j\right)}^T{\left(R_{k+1}^j\right)}^{-1}{H}_{k+1}^j\right)}^{-1}\underset{j=1}{\overset{q}{.Math.}}{{\gamma }_{k+1}^{j\left(t\right)}\left(H_{k+1}^j\right)}^T{\left(R_{k+1}^j\right)}^{-1}\left(Z_{k+1}^j-h_{k+1}^j\left({\hat{X}}_{k+1/k+1}^t\right)-H_{k+1}^j\left({\hat{X}}_{k+1/k}-{\hat{X}}_{k+1/k+1}^{t+1}\right)\right)={\hat{X}}_{k+1/k}+\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}\left(Z_{k+1}^j-h_{k+1}^j\left({\hat{X}}_{k+1/k+1}^t\right)-H_{k+1}^j\left({\hat{X}}_{k+1/k}-{\hat{X}}_{k+1/k+1}^{t+1}\right)\right)& \left(40\right)\end{array}$

[0080] In the above equation, the filter gain is:

[00023]$\begin{array}{cc}{K}_{k+1}^{j\left(t\right)}={\left({\xi }_{k+1}^t{\hat{P}}_{k+1/k}^1+\underset{i=1}{\overset{q}{.Math.}}{{\gamma }_{k+1}^{i\left(t\right)}\left(H_{k+1}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}{H}_{k+1}^i\right)}^{-1}{{\gamma }_{k+1}^{j\left(t\right)}\left(H_{k+1}^j\right)}^T{\left(R_{k+1}^j\right)}^{-1}& \left(41\right)\end{array}$

[0081] where ξ.sub.k+1.sup.t and γ.sub.k+1.sup.i(t) are equivalent to auxiliary parameters used for adjusting the one-step prediction covariance matrix and the measurement noise variance.

[0082] The estimated error is:

[00024]$\begin{array}{cc}\delta X_{k+1}^t=X_{k+1}-{\hat{X}}_{k+1/k+1}^t=X_{k+1}-{\hat{X}}_{k+1/k}-\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}\left(Z_{k+1}^j-h_{k+1}^j\left({\hat{X}}_{k+1/k+1}^t\right)-H_{k+1}^j\left({\hat{X}}_{k+1/k}-{\hat{X}}_{k+1/k+1}^t\right)\right)\approx \left(I-\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}{H}_{k+1}^j\right)\left(X_{k+1}-{\hat{X}}_{k+1/k}\right)-\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}{v}_{k+1}^j.& \left(42\right)\end{array}$

[0083] From this, the estimated error covariance matrix can be directly obtained:

[00025]$\begin{array}{cc}{\hat{P}}_{k+1/k+1}^t=E\left[\delta X_{k+1}^t\delta X_{k+1}^{tT}\right]=\left(I-\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}{H}_{k+1}^j\right){{\hat{P}}_{k+1/k}\left(I-\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}{H}_{k+1}^j\right)}^T+\underset{j=1}{\overset{q}{.Math.}}{K}_{k+1}^{j\left(t\right)}{R_{k+1}^j\left(K_{k+1}^{j\left(t\right)}\right)}^T.& \left(43\right)\end{array}$

[0084] Typically, for a SINS/DVL tight integrated navigation system, the process noise caused by random drift of a gyro and an accelerometer is a Gaussian noise with precise covariance. Therefore, the similarity function ƒ.sub.x(t) is selected to be ƒ.sub.x(t)=−0.5t, that is, the similarity between the state and the predicted state is measured by the negative mean squared error. The beam measurement noise of the DVL may be a non-Gaussian noise caused by outliers, and therefore, the similarity function ƒ.sub.z(t) is selected to be ƒ.sub.z(t)=−√{square root over ((ω+m)(ω+t))}. ωis the dof parameter, and m is the dimension of measurement, which is 1 in the disclosure.

[0085] Step 5: Feedback of the estimated navigation error to the SINS

[0086] Through a robust Kalman filter, the estimated misalignment angle ϕ, velocity error δV.sup.n and position error δp may be obtained. The attitude matrix, velocity and position solved by pure inertance of the SINS are assumed to be C.sub.b.sup.n′, {circumflex over (V)}.sup.n, and {circumflex over (p)}. Since the strap-down inertial navigation is corrected by feedback correction, the error of the inertial navigation is always kept small, so the feedback correction method of the attitude error is:

C.sub.b.sup.n=(I+ϕx)C.sub.b.sup.n′  (44).

[0087] The velocity error δV.sup.n and position error δp are subtracted directly from the output of the SINS.

V.sup.n={circumflex over (V)}.sup.n−δV.sup.n,p={circumflex over (p)}−δp   (45).

[0088] Step 6: Verification of the proposed algorithm using experimental data of a lake trial

[0089] FIG. 3 shows a navigation trajectory of a lake trial. In the experiment, a self-developed strap-down inertial navigation system and DVL are used as devices to be tested, and a GPS receiver and a PHINS inertial navigation system imported from France are used as the benchmark.

[0090] FIG. 4 shows the velocity error and horizontal positioning error of the proposed algorithm and the existing robust filtering algorithm applied to the SINS/DVL tightly integrated system. RSTKF represents an existing robust Kalman filter based on Student's t distribution modeling. MCKF represents an existing Kalman filter based on the maximum correntropy criterion. SSMKF represents an existing Kalman filter based on a statistical similarity measure. The biggest difference between the SSMKF and the robust Kalman filter proposed by the disclosure is that SSMKF only uses one auxiliary parameter to uniformly adjust the utilization rate of all measurements, while the robust Kalman filter proposed by the disclosure uses a plurality of auxiliary parameters to adjust the utilization rates of different measurement components respectively. From FIG. 4, it can be seen that both the positioning error and velocity error of the proposed algorithm are the smallest, and the estimation accuracy of the proposed algorithm is obviously higher than that of the SSMKF based on the same statistical similarity measure criterion.

[0091] FIG. 5 shows the changes of the measured value of a DVL beam and the auxiliary parameters corresponding to each beam in the experiment. From the figure, it can be seen that when there are large outliers, the auxiliary parameters corresponding to the DVL beams are significantly reduced, which increases the measurement noise variances of the beams and reduces the proportion of large outliers in the estimation results. However, the auxiliary parameters corresponding to normal DVL beams do not change significantly, so that the proposed algorithm can ensure use of normal measurements while reducing the influence of erroneous measurements.

[0092] In view of the situation that there are normal beam measurement information of the DVL and beam measurement information with a large error simultaneously in a SINS/DVL tightly integrated system, and aiming at the problem that the existing robust filters of an integrated navigation system process the measurement information in a rough manner and are likely to lead to loss of normal measurement information, the disclosure proposes a novel robust filtering method based on decomposition of multi-dimensional measurement equations and a statistical similarity measure.

[0093] The disclosure introduces the SSM theory while decomposing the multi-dimensional measurement equations of the SINS/DVL tightly integrated navigation system, and assists the measurement noise variance of each beam to complete respective adaptive update in case of a large measurement error, finally ensuring independence of processing of the measurement information of each beam. The disclosure can be used in the field of integrated navigation of underwater vehicles under non-ideal conditions.

[0094] The main advantages of the disclosure are as follows:

[0095] (1) The disclosure decomposes the measurement equations of a SINS/DVL tightly integrated navigation system, establishes a cost function using the statistical similarity measure concept, and improves the utilization efficiency of normal measurement information by a robust information fusion method of the integrated navigation system in the case of abnormal measurement.

[0096] (2) The disclosure iteratively solves the nonlinear cost function, reducing the numerical calculation error.

[0097] (3) The disclosure considers the system as a nonlinear system when establishing and solving the cost function, therefore, the robust information fusion method proposed by the disclosure has a broad range of application, and can be used in both linear systems and nonlinear systems.

[0098] The above embodiments are merely preferred embodiments of the disclosure and are not intended to limit the disclosure. It will be apparent to those skilled in the art that various modifications and changes may be made in the disclosure. Any modification, equivalent replacement and improvement made within the spirit and principle of the disclosure shall be included within the protection scope of the disclosure.