CN101639365A - Offshore alignment method of autonomous underwater vehicle based on second order interpolating filter - Google Patents

Abstract

The invention provides an offshore alignment method of an autonomous underwater vehicle based on a second order interpolating filter, comprising the following steps of: (1) collecting data output by an optical fiber gyroscope and a quartz flexible accelerometer, and primarily ensuring transfer matrix Cb<c> from a carrier coordinate system to a computing navigation coordinate system; (2) building anonlinear state equation which takes speed error and three error misalignment angles as state variable and a linear observation equation which takes speed error as observed quantity when a fiber optic gyro strap-down inertial navigation system has a lager azimuth error angle; (3) discretizing a nonlinear continuous system, filtering the discretized nonlinear system with the second order interpolating filter, and estimating the azimuth error angle; and (4) error compensating attitude matrix Cb<c> with the estimated azimuth error angle to obtain the exact attitude matrix, obtaining carrier attitude according to the attitude matrix, completing initial alignment and entering into navigation. The method can meet requirement with higher applicable precision, has high alignment precision and short alignment time, and is easily realized.

Description

Based on the marine alignment methods of the autonomous type underwater hiding-machine of second order interpolation wave filter

(1) technical field

What the present invention relates to is a kind of measuring method, the initial alignment technology that particularly relates to a kind of strapdown inertial navigation system relates in particular to a kind of autonomous type underwater hiding-machine fiber-optic gyroscope strapdown inertial navigation system and breaks away from the bigger alignment methods in coarse alignment azimuthal error angle when lash ship is marine to be started.

(2) background technology

Autonomous type underwater hiding-machine (Autonomous Underwater Vehicle, be called for short AUV) a kind of novel carrying platform under water that to be the beginning of the nineties at the end of the eighties developed rapidly on manned latent device and nobody have the technical foundation of cable telecontrolled submergence rescue vehicle (ROV) meter.AUV method such as unrenewable Transfer Alignment after breaking away from lash ship realizes the initial alignment of self, therefore seek a kind of can rapid alignment and guarantee that the alignment methods of certain precision becomes a difficult point of AUV research.Because the AUV own vol is little, in light weight, after breaking away from lash ship, it is subjected to the influence of external environments such as stormy waves very big, therefore the coarse alignment precision of AUV is very low, and traditional little misalignment AUV strapdown inertial navitation system (SINS) (SINS) linear error model is only just set up under the less condition of the various error sources of hypothesis.Strapdown inertial navitation system (SINS) initial alignment method all is based on the error model of system.Lot of documents and engineering practice prove that for the AUV fiber-optic gyroscope strapdown inertial navigation system, when the azimuthal error angle was low-angle, it is linear that the error model of aligning is approximately, and can carry out state estimation with Kalman filtering.When the azimuthal error angle is wide-angle, the error model of aiming at is non-linear, and aspect nonlinear filtering, more widely used method has two, the one, with EKF (EKF) be representative that nonlinear function is carried out linearization is approximate, utilize discrete nonlinear equation around filter value

Be launched into Taylor series, omit above of second order, obtain the linearization of nonlinear system model, but this filtering method has been introduced the high-order truncation error, estimated accuracy is lower; Another is based on the Filtering Estimation method of approximate probability density distribution, UKF is exactly wherein a kind of, UKF is by constructing average and the variance that one group of sampled point comes the capture systems state, each sampled point all passes through the conversion of nonlinear function, the average of system state prediction and variance are provided by the sampled point after changing, sampled point how much determined the filtering time, under identical sample frequency, estimated time is long.Therefore improve the initial alignment precision of AUV fiber-optic gyroscope strapdown inertial navigation system under the moving big orientation of pedestal misalignment, there is crucial meaning the shortening initial alignment time.

(3) summary of the invention

The object of the present invention is to provide a kind of requirement that can reach higher suitable precision, break away from the marine startup of lash ship, alignment precision height, short, the marine alignment methods of simple relatively the autonomous type underwater hiding-machine of realization of aligning time based on the second order interpolation wave filter for the AUV strapdown inertial navitation system (SINS).

The object of the present invention is achieved like this:

(1) gathers the data that fibre optic gyroscope and quartz flexible accelerometer are exported after the fiber-optic gyroscope strapdown inertial navigation system preheating, tentatively determine the transition matrix C of carrier coordinate system with the relation of earth rotation angle speed to calculating navigation coordinate system according to the output of accelerometer and the relation and the gyroscope output of acceleration of gravity _b ^c

The nonlinear state equation that is state variable with velocity error and three error misalignments when (2) setting up fiber-optic gyroscope strapdown inertial navigation system bigger at the azimuthal error angle (greater than 5 °) and be the linear observation equation of observed quantity with the velocity error;

(3), discrete nonlinear system is carried out filtering, the evaluated error misalignment with second order interpolation (Second-order Divided Difference is called for short DD2) wave filter with nonlinear continuous system discretize;

(4) utilize error misalignment that step (3) estimates to obtain to Matrix C _b ^cCarry out error compensation, obtain accurate attitude matrix, obtain attitude of carrier, finish initial alignment, enter navigation according to attitude matrix.

The present invention can also comprise following feature:

1, the described nonlinear state equation that to set up fiber-optic gyroscope strapdown inertial navigation system be state variable with velocity error and three error misalignments at the azimuthal error angle when bigger and be that the linear observation equation of observed quantity comprises with the velocity error:

The nonlinear state equation is: $\stackrel{\&CenterDot;}{x}=f\left|x\right|+\mathrm{Bw}$

Wherein: $x={\left[\begin{array}{cccccccccc}{\mathrm{\&Delta;V}}_x& {\mathrm{\&Delta;V}}_y& {\mathrm{\&phi;}}_x& {\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_z& {\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T$

$w={\left[\begin{array}{cccccccccc}{\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& 0& 0& 0& 0& 0\end{array}\right]}^T$

$B\left(\mathrm{1,1}\right)=C_b^c\left(\mathrm{1,1}\right),$ $B\left(\mathrm{1,2}\right)=C_b^c\left(\mathrm{1,2}\right),$ $B\left(\mathrm{2,1}\right)=C_b^c\left(\mathrm{2,1}\right),$ $B\left(\mathrm{2,2}\right)=C_b^c\left(\mathrm{2,2}\right),$ $B\left(\mathrm{3,3}\right)=-C_b^c\left(\mathrm{1,1}\right),$ $B\left(\mathrm{3,4}\right)={-C}_b^c\left(\mathrm{1,2}\right),$ $B\left(\mathrm{3,5}\right)=-C_b^c\left(\mathrm{1,3}\right),$ $B\left(\mathrm{4,3}\right)=-C_b^c\left(\mathrm{2,1}\right),$ $B\left(\mathrm{4,4}\right)=-C_b^c\left(\mathrm{2,2}\right),$ $B\left(\mathrm{4,5}\right)={-C}_b^c\left(\mathrm{2,3}\right),$ $B\left(\mathrm{5,3}\right)=-C_b^c\left(\mathrm{3,1}\right),$ $B\left(\mathrm{5,4}\right)=-C_b^c\left(\mathrm{3,2}\right),$ $B\left(\mathrm{5,5}\right)=-C_b^c\left(\mathrm{3,3}\right),$ Other element is 0 among the B;

Wherein, Δ V _x, Δ V _yBe of the projection of the error of inertial navigation system computing velocity and true velocity in Department of Geography; φ _x, φ _y, φ _zBe three error misalignments;

Be of the projection of acceleration calculation system deviation in geographic coordinate system; ε _x, ε _y, ε _zBe x, y, the gyroscopic drift of z axle; V _x ^t, V _y ^tBe the true velocity of carrier at geographic coordinate system x, y axle; L is a local latitude; F ' _x, f ' _yBe the reading of accelerometer at x, y axle; ω _IeBe earth rotation angle speed;

Wherein: R _eBe equatorial radius, R _pBe the pole axis radius; E=1/297;

Be geographic longitude;

Linear observation equation is: z=Cx+v

Wherein: C=[1 10000000 0] ^T, $v=\left[\begin{array}{cc}{\&dtri;}_x& {\&dtri;}_y\end{array}\right];$

With state equation and observation equation discretize:

x _k+1＝f(x _k，w _k)

y _k＝g(x _k，v _k)

Wherein: x _k∈ R ¹⁰, be 10 * 1 state vector; y _k∈ R ², be 2 * 1 observation vector; w _k∈ R ¹⁰, be 10 * 1 state-noise vector; v _k∈ R ², be 2 * 1 observation noise vector; According to the big misalignment nonlinearity erron of AUV strapdown inertial navitation system (SINS) engine-bed equation, the state-noise of supposing the system and observation noise are uncorrelated Gaussian white noises, and their average and covariance are:

E(w _k)＝w _k，E{(w _k-w)(w _j-w)}＝Q _k

E(v _k)＝v _k，E{(v _k-v)(v _j-v)}＝R _k

S _x, S _wAnd S _vIt is respectively the covariance prediction Covariance P _k, the Cholesky of state-noise covariance average Q and observation noise covariance R decomposes,

${\hat{P}}_k={\hat{S}}_x{\hat{S}}_x^T,$ P _k＝S _xS _x ^T，Q＝S _wS _w ^T，R＝S _vS _v ^T。

2, with the second order interpolation wave filter discrete nonlinear system is carried out filtering, the step of evaluated error misalignment is as follows:

1) the average x of init state vector x ₀With covariance P ₀Cholesky decompose S _x(0);

2) ask output y _kFirst order difference

, S _Yv ⁽¹⁾And second order difference

(k), S _Yv ⁽²⁾(k),

$S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{g_i({\stackrel{\&OverBar;}{x}}_k+h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)-g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)\}$

$S_{\mathrm{yv}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+h{S}_{v,j})-g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-h{S}_{v,j})\}$

$S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{{2h}^2}\{g_i({\stackrel{\&OverBar;}{x}}_k+h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)+g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)-{2g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)\}$

$S_{\mathrm{yv}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{{2h}^2}\{g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+{\mathrm{hS}}_{v,j})+g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-{\mathrm{hS}}_{v,j})-{2g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)\}$

Wherein, Be

J row, S _{V, j}Be S _vJ row; Step-length is made as ${}^{\left[7\right]}h=\sqrt{3};$

3) ask output y _kCalculated value y _k,

${\stackrel{\&OverBar;}{y}}_k=\frac{h^2-n_x-n_w}{h^2}{g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)+\frac{1}{{2\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="A2009100725640017H2.png"\; state="\{\{state\}\}">h</figure-callout>}}^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}{g}_i({\stackrel{\&OverBar;}{x}}_k+h{\stackrel{\&OverBar;}{S}}_{x,p},{\stackrel{\&OverBar;}{v}}_k)+g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,p},{\stackrel{\&OverBar;}{v}}_k)+$

$\frac{1}{{2\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="A2009100725640017H2.png"\; state="\{\{state\}\}">h</figure-callout>}}^2}\underset{p=1}{\overset{n_w}{\mathrm{\&Sigma;}}}{g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+{\mathrm{hS}}_{v,p})+g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-{\mathrm{hS}}_{v,p})$

N wherein _x=3, n _w=3;

4) ask output y _kThe Cholesky of covariance decompose S _y(k),

$S_y\left(k\right)=\left[\begin{array}{cccc}{S}_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)& S_{\mathrm{yv}}^{\left(1\right)}\left(k\right)& S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}\left(k\right)& S_{\mathrm{yv}}^{\left(2\right)}\left(k\right)\end{array}\right];$

5) ask filter gain K _kAnd P _Xy(k),

$P_{\mathrm{xy}}\left(k\right)={\stackrel{\&OverBar;}{S}}_x\left(k\right){\left(S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)\right)}^T$

K _k＝P _xy(k)(S _y(k)S _y(k) ^T) ^-1；

6) ask the prediction of state x

${\hat{x}}_k={\stackrel{\&OverBar;}{x}}_k+K_k(y_k-{\stackrel{\&OverBar;}{y}}_k);$

7) ask the covariance prediction

Cholesky decompose

${\hat{S}}_k\left(k\right)=\left[\begin{array}{cccc}{\stackrel{\&OverBar;}{S}}_x\left(k\right)-K_k{S}_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)& K_k{S}_{\mathrm{yv}}^{\left(1\right)}\left(k\right)& K_k{S}_{\mathrm{yx}}^{\left(2\right)}\left(k\right)& K_k{S}_{\mathrm{yv}}^{\left(2\right)}\left(k\right)\end{array}\right];$

8) computing mode prediction First order difference

S _Xw ⁽¹⁾And second order difference

And S _Xw ⁽²⁾,

$S_{x\hat{x}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{f_i({\hat{x}}_k+h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-f_i({\hat{x}}_k-h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)\}$

$S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+h{S}_{w,j})-f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-h{S}_{w,j})\}$

$S_{x\hat{x}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{{2h}^2}\{f_i({\hat{x}}_k+h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-f_i({\hat{x}}_k-h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-{2f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)$

$S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{{2h}^2}\{f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+{\mathrm{hS}}_{w,j})-f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-{\mathrm{hS}}_{w,j})-{2f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)\};$

9) ask x _K+1,

${\stackrel{\&OverBar;}{x}}_{k+1}=\frac{h^2-n_x-n_w}{h^2}{f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)+\frac{1}{{2\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="A2009100725640017H2.png"\; state="\{\{state\}\}">h</figure-callout>}}^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}{f}_i({\hat{x}}_k+h{\hat{S}}_{w,p},{\stackrel{\&OverBar;}{w}}_k)+f_i({\hat{x}}_k-h{\hat{S}}_{w,p},{\stackrel{\&OverBar;}{w}}_k)+$

$\frac{1}{{2\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="A2009100725640017H2.png"\; state="\{\{state\}\}">h</figure-callout>}}^2}\underset{p=1}{\overset{n_w}{\mathrm{\&Sigma;}}}{f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+h{S}_{w,p})+f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-h{S}_{w,p})$

Wherein,

Be

P row, S _{W, p}Be S _wP row;

10) ask state x _K+1Covariance P _K+1Cholesky decompose S _K+1,

${\stackrel{\&OverBar;}{S}}_k(k+1)=\left[\begin{array}{cccc}{S}_{x\hat{x}}^{\left(1\right)}\left(k\right)& S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)& S_{x\hat{x}}^{\left(2\right)}\left(k\right)& S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)\end{array}\right];$

11) k is increased to k+1 since 2) carry out.

Method of the present invention has following advantage: the shortcoming that Transfer Alignment and traditional alignment methods based on the linear error model no longer were suitable for when (1) had overcome marine startup of AUV disengaging lash ship, under the condition that does not increase system cost, can reach the requirement of higher suitable precision.(2) with based on the EKF of Taylor expansion and different based on the UKF of approximate probability density distribution, overcome the shortcoming of EKF introducing high-order truncation error based on the second order interpolation filtering method of multidimensional Stirling interpolation formula, also overcome the big shortcoming of UKF calculated amount, break away from the marine startup of lash ship for the AUV strapdown inertial navitation system (SINS), the alignment precision height, the aligning time is short, realizes simple.

Beneficial effect of the present invention is described as follows:

In order to verify practicality of the present invention, carried out the Matlab emulation experiment.

Under medium sea situation, AUV three-axis swinging amplitude is: 6 °, 5 ° and 2 °;

AUV speed: 8 joints;

The constant value drift of gyro is: ε _Bi=0.01 °/h, random walk coefficient in angle is:

The scale factor error is: δ K _Gi=10ppm;

The normal value of accelerometer is biased to: ${\&dtri;}_{\mathrm{bi}}=1\&times;{10}^{-4}g,$ The measurement white noise is: ${\mathrm{\&sigma;}}_{{\&dtri;}_{\mathrm{wi}}}=1\&times;{10}^{-5}g,$ The scale factor error is: δ K _Ai=10ppm;

Initial position is: longitude is 126.6705 °, and latitude is 45.7796 °;

The starting condition of misalignment: φ (0)=[1 ° 1 ° 10 °] ^T

The interpolation filter initial value:

x ₀＝[0?0?0?0?0?0?0?0?0?0] ^T

S _x(0)＝diag{0.1?0.1?1°?1°?10°?10 ^-4g?10 ^-4g?0.01°/h 0.01°/h 0.01°/h}

Under above experiment condition, simulation result is as shown in the table:

Filtering method misalignment estimated time φ _zEvaluated error

EKF 293s 12.4′

UKF 491s 4.1′

DD2 353s 4.9′

(4) description of drawings

Fig. 1 structured flowchart of the present invention;

Fig. 2 DD2 wave filter process flow diagram;

Fig. 3 EKF orientation misalignment φ _zEvaluated error;

Fig. 4 UKF orientation misalignment φ _zEvaluated error;

Fig. 5 DD2 wave filter orientation misalignment φ _zEvaluated error.

(5) embodiment

For example the present invention is done description in more detail below in conjunction with accompanying drawing:

The present invention includes the following step:

(1) gathers the data that fibre optic gyroscope and quartz flexible accelerometer are exported after the fiber-optic gyroscope strapdown inertial navigation system preheating, export the attitude matrix C that tentatively determines this moment with the relation of earth rotation angle speed according to the output of accelerometer and the relation and the gyroscope of acceleration of gravity _b ^c(this moment, the lateral error angle was a low-angle, and the azimuthal error angle is bigger).

The nonlinear state equation that is state variable with velocity error and three error misalignments when (2) setting up fiber-optic gyroscope strapdown inertial navigation system bigger at the azimuthal error angle (greater than 5 °) and be the linear observation equation of observed quantity with the velocity error.

1) set up the velocity error equation:

The velocity error equation is:

$\mathrm{\&Delta;}{\stackrel{\&CenterDot;}{V}}_x=-(\cos {\mathrm{\&phi;}}_z-1){\stackrel{\&OverBar;}{f}}_x^{\&prime;}+\left(\sin {\mathrm{\&phi;}}_{\text{z}}\right){\stackrel{\&OverBar;}{f}}_y^{\&prime;}-g({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}\left(\sin L\right){\mathrm{\&Delta;V}}_y+{\&dtri;}_x$ (1)

$\mathrm{\&Delta;}{\stackrel{\&CenterDot;}{V}}_y=-\left(\sin {\mathrm{\&phi;}}_z\right){\stackrel{\&OverBar;}{f}}_x^{\&prime;}-(\cos {\mathrm{\&phi;}}_{\text{z}}-1){\stackrel{\&OverBar;}{f}}_y^{\&prime;}+g({-\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\left(\sin L\right){\mathrm{\&Delta;V}}_x+{\&dtri;}_y$

Wherein: L is a local latitude;

Be of the projection of acceleration calculation system deviation in geographic coordinate system; Δ V _x, Δ V _yVelocity error is in the projection of Department of Geography; F ' _x, f ' _yBe the reading of accelerometer at the corresponding coordinate axle; ω _IeBe earth rotation angle speed.

2) set up error misalignment equation;

The error misalignment is:

φ _t＝[φ _x?φ _y?φ _z] ^T (2)

The differential equation of error misalignment is:

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_x=-(1-\cos {\mathrm{\&phi;}}_z)\frac{V_y^t}{R_{\mathrm{yt}}}-\left(\sin {\mathrm{\&phi;}}_z\right)({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x^t}{R_{\mathrm{xt}}}{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)+{\mathrm{\&phi;}}_y({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x^t}{R_{\mathrm{xt}}}\mathrm{tgL})-\frac{{\mathrm{\&Delta;V}}_y}{R_{\mathrm{yt}}}+{\mathrm{\&epsiv;}}_x$

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_y=-\left(\sin {\mathrm{\&phi;}}_z\right)\frac{V_y^t}{R_{\mathrm{yt}}}+(1-\cos {\mathrm{\&phi;}}_z)({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x^t}{R_{\mathrm{xt}}}{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)-{\mathrm{\&phi;}}_x({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x^t}{R_{\mathrm{xt}}}\mathrm{tgL})+\frac{{\mathrm{\&Delta;V}}_x}{R_{\mathrm{xt}}}+{\mathrm{\&epsiv;}}_y---\left(3\right)$

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_z=({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)\frac{V_y^t}{R_{\mathrm{yt}}}+(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x^t}{R_{\mathrm{xt}}}{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)+\frac{{\mathrm{\&Delta;V}}_x}{R_{\mathrm{xt}}}\mathrm{tgL}+{\mathrm{\&epsiv;}}_z$

Wherein: ε _x, ε _y, ε _zBe the projection of gyroscopic drift in Department of Geography; V _x ^t, V _t ^yBe the true velocity of carrier in geographic coordinate system; Δ V _x, Δ V _yIt is the error of inertial navigation system computing velocity and true velocity;

(4)

Wherein: R _eBe equatorial radius, R _pBe the pole axis radius;

$e=\frac{R_e-R_p}{R_e}=\frac{1}{297}---\left(5\right)$

3) set up the nonlinear state equation and the observation equation of fiber-optic gyroscope strapdown inertial navigation system:

$\stackrel{\&CenterDot;}{x}=f\left|x\right|+\mathrm{Bw}---\left(6\right)$

z＝Cx+v

Wherein:

$x={\left[\begin{array}{cccccccccc}{\mathrm{\&Delta;V}}_x& {\mathrm{\&Delta;V}}_y& {\mathrm{\&phi;}}_x& {\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_z& {\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T---\left(7\right)$

z＝[Δv _x?Δv _y] ^T (8)

$B\left(\mathrm{1,1}\right)=C_b^c\left(\mathrm{1,1}\right),$ $B\left(\mathrm{1,2}\right)=C_b^c\left(\mathrm{1,2}\right),$ $B\left(\mathrm{2,1}\right)=C_b^c\left(\mathrm{2,1}\right),$ $B\left(\mathrm{2,2}\right)=C_b^c\left(\mathrm{2,2}\right),$ $B\left(\mathrm{3,3}\right)=-C_b^c\left(\mathrm{1,1}\right),$ $B\left(\mathrm{3,4}\right)={-C}_b^c\left(\mathrm{1,2}\right),$ $B\left(\mathrm{3,5}\right)=-C_b^c\left(\mathrm{1,3}\right),$ $B\left(\mathrm{4,3}\right)=-C_b^c\left(\mathrm{2,1}\right),$ $B\left(\mathrm{4,4}\right)=-C_b^c\left(\mathrm{2,2}\right),$ $B\left(\mathrm{4,5}\right)={-C}_b^c\left(\mathrm{2,3}\right),$ $B\left(\mathrm{5,3}\right)=-C_b^c\left(\mathrm{3,1}\right),$ $B\left(\mathrm{5,4}\right)=-C_b^c\left(\mathrm{3,2}\right),$ $B\left(\mathrm{5,5}\right)=-C_b^c\left(\mathrm{3,3}\right),$ Other element is 0 among the B.

$w={\left[\begin{array}{cccccccccc}{\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& 0& 0& 0& 0& 0\end{array}\right]}^T---\left(10\right)$

C＝[1?1?0?0?0?0?0?0?0?0] ^T (11)

$v=\left[\begin{array}{cc}{\&dtri;}_x& {\&dtri;}_y\end{array}\right]---\left(12\right)$

With state equation and observation equation discretize:

x _k+1＝f(x _k，w _k) (13)

y _k＝g(x _k，v _k)

Wherein: x _k∈ R ¹⁰, be 10 * 1 state vector; y _k∈ R ², be 2 * 1 observation vector; w _k∈ R ¹⁰, be 10 * 1 state-noise vector; v _k∈ R ², be 2 * 1 observation noise vector.According to the big misalignment nonlinearity erron of AUV strapdown inertial navitation system (SINS) engine-bed equation, the state-noise of supposing the system and observation noise are uncorrelated Gaussian white noises, and their average and covariance are:

E(w _k)＝w _k，E{(w _k-w)(w _j-w)}＝Q _k

(14)

E(v _k)＝v _k，E{(v _k-v)(v _j-v)}＝R _k

(3) with the DD2 wave filter AUV fiber-optic gyroscope strapdown inertial navigation system is carried out Filtering Estimation, the evaluated error misalignment.

DD2 wave filter evaluated error misalignment step is as follows:

1) the average x of init state vector x ₀With covariance P ₀Cholesky decompose S _x(0);

2) ask output y _kFirst order difference

, S _Yv ⁽¹⁾And second order difference

(k), S _Yv ⁽²⁾(k);

$S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{g_i({\stackrel{\&OverBar;}{x}}_k+h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)-g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)\}$ (15)

$S_{\mathrm{yv}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+h{S}_{v,j})-g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-h{S}_{v,j})\}$

$S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{{2h}^2}\{g_i({\stackrel{\&OverBar;}{x}}_k+h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)+g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)-{2g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)\}$ (16)

$S_{\mathrm{yv}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{{2h}^2}\{g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+{\mathrm{hS}}_{v,j})+g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-{\mathrm{hS}}_{v,j})-{2g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)\}$

Wherein,

Be J row, S _{V, j}Be S _vJ row; Step-length is made as ${}^{\left[7\right]}h=\sqrt{3}.$

3) ask output y _kCalculated value y _k

${\stackrel{\&OverBar;}{y}}_k=\frac{h^2-n_x-n_w}{h^2}{g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)+\frac{1}{{2\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="A2009100725640017H2.png"\; state="\{\{state\}\}">h</figure-callout>}}^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}{g}_i({\stackrel{\&OverBar;}{x}}_k+{h\stackrel{\&OverBar;}{S}}_{x,p},{\stackrel{\&OverBar;}{v}}_k)+g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,p},{\stackrel{\&OverBar;}{v}}_k)+$ (17)

$\frac{1}{{2\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="A2009100725640017H2.png"\; state="\{\{state\}\}">h</figure-callout>}}^2}\underset{p=1}{\overset{n_w}{\mathrm{\&Sigma;}}}{g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+{\mathrm{hS}}_{v,p})+g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-{\mathrm{hS}}_{v,p})$

N wherein _x=3, n _w=3.

4) ask output y _kThe Cholesky of covariance decompose S _y(k);

$S_y\left(k\right)=\left[\begin{array}{cccc}{S}_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)& S_{\mathrm{yv}}^{\left(1\right)}\left(k\right)& S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}\left(k\right)& S_{\mathrm{yv}}^{\left(2\right)}\left(k\right)\end{array}\right]---\left(18\right)$

5) ask filter gain K _kAnd P _Xy(k);

$P_{\mathrm{xy}}\left(k\right)={\stackrel{\&OverBar;}{S}}_x\left(k\right){\left(S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)\right)}^T---\left(19\right)$

K _k＝P _xy(k)(S _y(k)S _y(k) ^T) ^-1 (20)

6) ask the prediction of state x

${\hat{x}}_k={\stackrel{\&OverBar;}{x}}_k+K_k(y_k-{\stackrel{\&OverBar;}{y}}_k)---\left(21\right)$

7) ask the covariance prediction

Cholesky decompose

${\hat{S}}_k\left(k\right)=\left[\begin{array}{cccc}{\stackrel{\&OverBar;}{S}}_x\left(k\right)-K_k{S}_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)& K_k{S}_{\mathrm{yv}}^{\left(1\right)}\left(k\right)& K_k{S}_{\mathrm{yx}}^{\left(2\right)}\left(k\right)& K_k{S}_{\mathrm{yv}}^{\left(2\right)}\left(k\right)\end{array}\right]---\left(22\right)$

8) computing mode prediction First order difference

S _Xw ⁽¹⁾And second order difference

And S _Sw ⁽²⁾

$S_{x\hat{x}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{f_i({\hat{x}}_k+h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-f_i({\hat{x}}_k-h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)\}$ (23)

$S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+h{S}_{w,j})-f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-h{S}_{w,j})\}$

$S_{x\hat{x}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{{2h}^2}\{f_i({\hat{x}}_k+h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-f_i({\hat{x}}_k-h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-{2f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)$ (24)

$S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{{2h}^2}\{f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+{\mathrm{hS}}_{w,j})-f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-{\mathrm{hS}}_{w,j})-{2f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)\}$

9) ask x _K+1

${\stackrel{\&OverBar;}{x}}_{k+1}=\frac{h^2-n_x-n_w}{h^2}{f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)+\frac{1}{{2\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="A2009100725640017H2.png"\; state="\{\{state\}\}">h</figure-callout>}}^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}{f}_i({\hat{x}}_k+h{\hat{S}}_{w,p},{\stackrel{\&OverBar;}{w}}_k)+f_i({\hat{x}}_k-h{\hat{S}}_{w,p},{\stackrel{\&OverBar;}{w}}_k)+$ (25)

$\frac{1}{{2\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="A2009100725640017H2.png"\; state="\{\{state\}\}">h</figure-callout>}}^2}\underset{p=1}{\overset{n_w}{\mathrm{\&Sigma;}}}{f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+h{S}_{w,p})+f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-h{S}_{w,p})$

Wherein,

Be

P row, S _{W, p}Be S _wP row;

10) ask state x _K+1Covariance P _K+1Cholesky decompose S _K+1

${\stackrel{\&OverBar;}{S}}_k(k+1)=\left[\begin{array}{cccc}{S}_{x\hat{x}}^{\left(1\right)}\left(k\right)& S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)& S_{x\hat{x}}^{\left(2\right)}\left(k\right)& S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)\end{array}\right]---\left(26\right)$

11) k is increased to k+1 since 2) carry out.

(4) utilize misalignment that step (3) estimates to obtain to attitude matrix C _b ^tCarry out error compensation, obtain accurate attitude matrix and obtain attitude of carrier, finish initial alignment, enter navigation according to attitude matrix.

Through interpolation filter filtering, estimate three error misalignment φ _x, φ _y, φ _z, to Matrix C _b ^tCompensate:

$C_b^{\&prime;}=C_c^{\&prime;}{C}_b^c---\left(27\right)$

Wherein:

$C_t^c=\left[\begin{array}{ccc}\cos {\mathrm{\&phi;}}_z& {\mathrm{\&phi;}}_z& {-\mathrm{\&phi;}}_y\\ -\sin {\mathrm{\&phi;}}_z& \cos {\mathrm{\&phi;}}_z& {\mathrm{\&phi;}}_x\\ {\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_x& 1\end{array}\right]---\left(28\right)$

According to C _b ^tObtain attitude of carrier, promptly the main value of pitch angle, roll angle and course angle is as follows:

As follows by the formula that above-mentioned three main values are judged true value:

θ=θ _Main(30)

So far, initial alignment is finished, and can enter navigational state.

Claims (3)

1, the marine alignment methods of a kind of autonomous type underwater hiding-machine based on the second order interpolation wave filter is characterized in that:

(1) gathers the data that fibre optic gyroscope and quartz flexible accelerometer are exported after the fiber-optic gyroscope strapdown inertial navigation system preheating, tentatively determine the transition matrix C of carrier coordinate system with the relation of earth rotation angle speed to calculating navigation coordinate system according to the output of accelerometer and the relation and the gyroscope output of acceleration of gravity _b ^c

(2) set up fiber-optic gyroscope strapdown inertial navigation system at the azimuthal error angle more greatly, the nonlinear state equation that is state variable during promptly greater than 5 ° and be the linear observation equation of observed quantity with the velocity error with velocity error and three error misalignments;

(3), discrete nonlinear system is carried out filtering, the evaluated error misalignment with the second order interpolation wave filter with nonlinear continuous system discretize;

(4) utilize error misalignment that step (3) estimates to obtain to Matrix C _b ^cCarry out error compensation, obtain accurate attitude matrix, obtain attitude of carrier, finish initial alignment, enter navigation according to attitude matrix.

2, the marine alignment methods of the autonomous type underwater hiding-machine based on the second order interpolation wave filter according to claim 1 is characterized in that: the described nonlinear state equation that to set up fiber-optic gyroscope strapdown inertial navigation system be state variable with velocity error and three error misalignments at the azimuthal error angle when bigger and be that the linear observation equation of observed quantity comprises with the velocity error:

The nonlinear state equation is: $\stackrel{\&CenterDot;}{x}=f\left|x\right|+\mathrm{Bw}$

Wherein: $x={\left[\begin{array}{cccccccccc}\mathrm{\&Delta;}{V}_x& \mathrm{\&Delta;}{V}_y& {\mathrm{\&phi;}}_x& {\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_z& {\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T$

$f\left|x\right|=\left[\begin{array}{c}-(\cos {\mathrm{\&phi;}}_z-1){\stackrel{\&OverBar;}{f}}_x^{\&prime;}+\left(\sin {\mathrm{\&phi;}}_z\right){\stackrel{\&OverBar;}{f}}_y^{\&prime;}-g({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)+2{\stackrel{\&OverBar;}{\mathrm{\&omega;}}}_{\mathrm{ie}}\left(\sin L\right)\mathrm{\&Delta;}{V}_y+{\&dtri;}_x\\ -\left(\sin {\mathrm{\&phi;}}_z\right){\stackrel{\&OverBar;}{f}}_x^{\&prime;}-(\cos {\mathrm{\&phi;}}_z-1){\stackrel{\&OverBar;}{f}}_y^{\&prime;}+g(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)-2{\stackrel{\&OverBar;}{\mathrm{\&omega;}}}_{\mathrm{ie}}\left(\sin L\right)\mathrm{\&Delta;}{V}_x+{\&dtri;}_y\\ -(1-\cos {\mathrm{\&phi;}}_z)\frac{V_y^t}{R_{\mathrm{yt}}}-\left(\sin {\mathrm{\&phi;}}_z\right)({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x^t}{R_{\mathrm{xt}}}{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)+{\mathrm{\&phi;}}_y({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x^t}{R_{\mathrm{xt}}}\mathrm{tgL})-\frac{{\mathrm{\&Delta;}V}_y}{R_{\mathrm{yt}}}+{\mathrm{\&epsiv;}}_x\\ -\left(\sin {\mathrm{\&phi;}}_z\right)\frac{V_y^t}{R_{\mathrm{yt}}}+(1-\cos {\mathrm{\&phi;}}_z)({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x^t}{R_{\mathrm{xt}}}{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)-{\mathrm{\&phi;}}_x({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_x^t}{R_{\mathrm{xt}}}\mathrm{tgL})+\frac{{\mathrm{\&Delta;V}}_x}{R_{\mathrm{xt}}}+{\mathrm{\&epsiv;}}_y\\ ({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)\frac{V_y^t}{R_{\mathrm{yt}}}+(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_x^t}{R_{\mathrm{xt}}}{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)+\frac{\mathrm{\&Delta;}{V}_x}{R_{\mathrm{xt}}}\mathrm{tgL}+{\mathrm{\&epsiv;}}_z\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]$

$w={\left[\begin{array}{cccccccccc}{\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& 0& 0& 0& 0& 0\end{array}\right]}^T$

$B\left(\mathrm{1,1}\right)=C_b^c\left(\mathrm{1,1}\right),B\left(\mathrm{1,2}\right)=C_b^c\left(\mathrm{1,2}\right),B\left(\mathrm{2,1}\right)=C_b^c\left(\mathrm{2,1}\right),B\left(\mathrm{2,2}\right)=C_b^c\left(\mathrm{2,2}\right),B\left(\mathrm{3,3}\right)=-C_b^c\left(\mathrm{1,1}\right),B\left(\mathrm{3,4}\right)=-C_b^c\left(\mathrm{1,2}\right),$

$B\left(\mathrm{3,5}\right)=-C_b^c\left(\mathrm{1,3}\right),B\left(\mathrm{4,3}\right)=-C_b^c\left(\mathrm{2,1}\right),B\left(\mathrm{4,4}\right)=-C_b^c\left(\mathrm{2,2}\right),B\left(\mathrm{4,5}\right)=-C_b^c\left(\mathrm{2,3}\right),B\left(\mathrm{5,3}\right)=-C_b^c\left(\mathrm{3,1}\right),$

$B\left(\mathrm{5,4}\right)=-C_b^c\left(\mathrm{3,2}\right),B\left(\mathrm{5,5}\right)=-C_b^c\left(\mathrm{3,3}\right),$ , in other element be 0;

Wherein, Δ V _x, Δ V _yBe of the projection of the error of inertial navigation system computing velocity and true velocity in Department of Geography; φ _x, φ _y, φ _zBe three error misalignments; Be of the projection of acceleration calculation system deviation in geographic coordinate system; ε _x, ε _y, ε _zBe x, y, the gyroscopic drift of z axle; V _x ^t, V _y ^tBe the true velocity of carrier at geographic coordinate system x, y axle; L is a local latitude; F ' _x, f ' _yBe the reading of accelerometer at x, y axle; ω _IeBe earth rotation angle speed;

Wherein: R _eBe equatorial radius, R _pBe the pole axis radius; E=1/297;

Be geographic longitude;

Linear observation equation is: z=Cx+v

Wherein: C=[1 10000000 0] ^T, $v=\left[\begin{array}{cc}{\&dtri;}_x& {\&dtri;}_y\end{array}\right];$

With state equation and observation equation discretize:

x _k+1＝f(x _k，w _k)

y _k＝g(x _k，v _k)

Wherein: x _k∈ R ¹⁰, be 10 * 1 state vector; y _k∈ R ², be 2 * 1 observation vector; w _k∈ R ¹⁰, be 10 * 1 state-noise vector; v _k∈ R ², be 2 * 1 observation noise vector; According to the big misalignment nonlinearity erron of AUV strapdown inertial navitation system (SINS) engine-bed equation, the state-noise of supposing the system and observation noise are uncorrelated Gaussian self noises, and their average and covariance are:

E(w _k)＝w _k，E{(w _k-w)(w _j-w)}＝Q _k

E(v _k)＝v _k，E{(v _k-v)(v _j-v)}＝R _k

S _x, S _wAnd S _vIt is respectively the covariance prediction Covariance P _k, the Cholesky of state-noise covariance average Q and observation noise covariance R decomposes,

${\hat{P}}_k={\hat{S}}_x{\hat{S}}_x^T,$ P _k＝S _xS _x ^T，Q＝S _wS _w ^T，R＝S _vS _v ^T。

3, the marine alignment methods of the autonomous type underwater hiding-machine based on the second order interpolation wave filter according to claim 1 and 2, it is characterized in that: with the second order interpolation wave filter discrete nonlinear system is carried out filtering, the step of evaluated error misalignment is as follows:

1) the average x of init state vector x ₀With covariance P ₀Cholesky decompose S _x(0);

2) ask output y _kFirst order difference

S _Yv ⁽¹⁾And second order difference

S _Yv ⁽²⁾(k),

$S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{g_i({\stackrel{\&OverBar;}{x}}_k+h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)-g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,j}{\stackrel{\&OverBar;}{v}}_k)\}$

$S_{\mathrm{yv}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+{\mathrm{hS}}_{v,j})-g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-{\mathrm{hS}}_{v,j})\}$

$S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{2h^2}\{g_i({\stackrel{\&OverBar;}{x}}_k+h{\stackrel{\&OverBar;}{S}}_{x,j},{\stackrel{\&OverBar;}{v}}_k)+g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,j}{\stackrel{\&OverBar;}{v}}_k)-{2g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)\}$

$S_{\mathrm{yv}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{2h^2}\{g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+{\mathrm{hS}}_{v,j})-g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-{\mathrm{hS}}_{v,j})-2g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)\}$

Wherein, Be J row, S _{V, j}Be S _vJ row; Step-length is made as ^[7] $h=\sqrt{3};$

3) ask output y _kCalculated value y _k,

${\stackrel{\&OverBar;}{y}}_k=\frac{h^2-n_x-n_w}{h^2}{g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k)+\frac{1}{{2h}^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}{g}_i({\stackrel{\&OverBar;}{x}}_k+h{\stackrel{\&OverBar;}{S}}_{x,p},{\stackrel{\&OverBar;}{v}}_k)+g_i({\stackrel{\&OverBar;}{x}}_k-h{\stackrel{\&OverBar;}{S}}_{x,p},{\stackrel{\&OverBar;}{v}}_k)+$

$\frac{1}{{2h}^2}\underset{p=1}{\overset{n_w}{\mathrm{\&Sigma;}}}{g}_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k+{\mathrm{hS}}_{v,p})+g_i({\stackrel{\&OverBar;}{x}}_k,{\stackrel{\&OverBar;}{v}}_k-{\mathrm{hS}}_{v,p})$

N wherein _x=3, n _w=3;

4) ask output y _kThe Cholesky of covariance decompose S _y(k),

$S_y\left(k\right)=\left[\begin{array}{cccc}{S}_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)& S_{\mathrm{yv}}^{\left(1\right)}\left(k\right)& S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}\left(k\right)& S_{\mathrm{yv}}^{\left(2\right)}\left(k\right)\end{array}\right];$

5) ask filter gain K _kAnd P _Xy(k),

$P_{\mathrm{xy}}\left(k\right)={\stackrel{\&OverBar;}{S}}_x\left(k\right){\left(S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)\right)}^T$

K _k＝P _xy(k)(S _y(k)S _y(k) ^T) ^-1；

6) ask the prediction of state x

${\hat{x}}_k={\stackrel{\&OverBar;}{x}}_k+K_k(y_k-{\stackrel{\&OverBar;}{y}}_k);$

7) ask the covariance prediction Cholesky decompose

(k),

${\hat{S}}_x\left(k\right)=\left[\begin{array}{cccc}{\stackrel{\&OverBar;}{S}}_x\left(k\right)-K_k{S}_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}\left(k\right)& K_k{S}_{\mathrm{yv}}^{\left(1\right)}\left(k\right)& K_k{S}_{\mathrm{yx}}^{\left(2\right)}\left(k\right)& K_k{S}_{\mathrm{yv}}^{\left(2\right)}\left(k\right)\end{array}\right];$

8) computing mode prediction

First order difference

And second order difference

$S_{x\hat{x}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{f_i({\hat{x}}_k+h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-f_i({\stackrel{\&OverBar;}{x}}_k-h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)\}$

$S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)=\frac{1}{2h}\{f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+h{S}_{w,j})-f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-{\mathrm{hS}}_{w,j})\}$

$S_{x\hat{x}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{2h^2}\{f_i({\hat{x}}_k+h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-f_i({\stackrel{\&OverBar;}{x}}_k-h{\hat{S}}_{x,j},{\stackrel{\&OverBar;}{w}}_k)-{2f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)\}$

$S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)=\frac{\sqrt{h^2-1}}{2h^2}\{f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+h{S}_{w,j})-f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-{\mathrm{hS}}_{w,j})-2f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)\};$

9) ask x _K+1,

${\stackrel{\&OverBar;}{x}}_{k+1}=\frac{h^2-n_x-n_w}{h^2}{f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k)+\frac{1}{{2h}^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}{f}_i({\hat{x}}_k+h{\hat{S}}_{w,p},{\stackrel{\&OverBar;}{w}}_k)+f_i({\hat{x}}_k-h{\hat{S}}_{w,p},{\stackrel{\&OverBar;}{w}}_k)+$

$\frac{1}{2h^2}\underset{p=1}{\overset{n_w}{\mathrm{\&Sigma;}}}{f}_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k+{\mathrm{hS}}_{w,p})+f_i({\hat{x}}_k,{\stackrel{\&OverBar;}{w}}_k-{\mathrm{hS}}_{w,p})$

Wherein,

Be

P row, S _{W, p}Be S _wP row;

10) ask state x _K+1Covariance P _K+1Cholesky decompose S _K+1,

${\stackrel{\&OverBar;}{S}}_x(k+1)=\left[\begin{array}{cccc}{S}_{x\hat{x}}^{\left(1\right)}\left(k\right)& S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)& S_{x\hat{x}}^{\left(2\right)}\left(k\right)& S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)\end{array}\right];$

11) k is increased to k+1 since 2) carry out.

Images