CN104374401A - Compensating method of gravity disturbance in strapdown inertial navigation initial alignment - Google Patents

Abstract

The invention belongs to the field of error compensation in initial alignment, and particularly relates to a compensating method of gravity disturbance in strapdown inertial navigation initial alignment. The method includes the steps: acquiring angular velocity data outputted by an optical fiber gyro and accelerated velocity data outputted by a quartz accelerometer; calculating a gravity disturbance value of an alignment site through a gravity potential, and compensating the accelerated velocity data outputted by the quartz accelerometer; completing coarse alignment with an analytical method, and selecting a wave filtering initial value; estimating a platform misalignment angle; and completing accurate initial alignment. According to the compensating method of gravity disturbance in strapdown inertial navigation initial alignment, the gravity disturbance value of the alignment site is calculated according to known longitude and latitude values and an EGM2008 model, the calculated gravity disturbance value is eliminated from the accelerometer, the output of the accelerometer after the gravity disturbance compensation is obtained, and simulation results show that the initial alignment accuracy can be improved after gravity disturbance compensation.

Description

The compensation method of gravity disturbance in a kind of inertial navigation initial alignment

Technical field

The invention belongs to initial alignment medial error and compensate field, be specifically related to the compensation method of gravity disturbance in a kind of inertial navigation initial alignment.

Background technology

Initial alignment is one of Crucial Technology of SINS.Initial alignment precision directly affects the operating accuracy of strapdown inertial navigation system.The fundamental purpose of strapdown inertial navigation system initial alignment sets up the initial value of attitude matrix, by initial alignment state-space model in initial alignment, utilizes Kalman filtering by initial misalignment state estimation out and in order to correct attitude matrix.Traditional alignment procedures comprises coarse alignment and two stages of fine alignment, first roughly estimates the size of misalignment with coarse alignment model, and then utilizes fine alignment model estimate the size of misalignment and realize fine alignment.The strict mathematical error model of strapdown inertial navigation system is one group of nonlinear differential equation, and the misalignment of actual coarse alignment is wide-angle under many circumstances, therefore adopts nonlinear model more can reflect error Propagation Property really.

When inertial navigation resolves usually be normal gravity, normal gravity the earth is approximately the rotation ellipsoid model of a uniform quality and the gravity value obtained; And reality is due to earth interior non-uniform mass, gravitational vector comprises normal gravity and gravity disturbance.Because different regions earth interior mass distribution is different, these gravity disturbances are changes on earth's surface or sea level.Inertial acceleration meter can not distinguish the tangential direction component of terrestrial gravitation and the horizontal acceleration of body, and therefore these gravity disturbances can cause inertial navigation system resolution error.In inertial navigation, using the ratio force vector of accelerometer measures carrier positions, is measurement point absolute acceleration than force vector with acceleration of gravity difference be: from the specific force recorded, compensate gravitational acceleration, just can obtain the absolute acceleration of carrier, then according to the relation of absolute acceleration and relative acceleration, inertial navigation system can try to achieve relative velocity, the position of carrier further.Obviously, if the acceleration of gravity on flight path a/W acceleration can not be reflected really, so will cause the error of measurement point acceleration, cause navigation calculation error further.Along with inertia device precision improves constantly the development with High Accuracy Inertial Navigation System demand, gravity disturbance becomes a main error source of High Accuracy Inertial initial alignment.The height of the precision of initial alignment directly has influence on the precision of inertial navigation, realize high-precision inertial navigation and be necessary to compensate the gravity disturbance in initial alignment.

Summary of the invention

The object of the invention is the precision in order to improve initial alignment, the gravity disturbance existed when compensating initial alignment, proposition earth gravity field model E GM2008 calculates gravity disturbance and the compensation method of gravity disturbance in the inertial navigation initial alignment compensated.

The object of the present invention is achieved like this:

(1) angular velocity data of fibre optic gyroscope output and the acceleration information of quartz accelerometer output is gathered;

(2) calculated the gravity disturbance value of aiming at place by gravity disturbance position, the output acceleration information of quartz accelerometer is compensated;

(3) adopt analytical method to complete coarse alignment, pass through initial matrix determine attitude information and pitch angle, roll angle, the course angle of carrier, wherein b represents carrier coordinate system, and n ' representative calculates navigational coordinate system;

(4) filtering initial value is chosen

${\hat{x}}_0=E\left[x_0\right],P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right],$

Wherein x ₀for the initial value of state variable, P ₀for the initial error covariance matrix of state variable;

(5) utilize Unscented kalman filtering method to carry out filtering, estimate the misaligned angle of the platform;

(6) the strapdown initial matrix of the misaligned angle of the platform update the system estimated is utilized obtain accurate initial strap-down matrix namely thus complete accurate initial alignment.

Gravity disturbance position T (ρ, L, λ),

$T(\mathrm{\ρ},L,\mathrm{\λ})=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{N}{\mathrm{\Σ}}}{\left(\frac{R}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[C_{\mathrm{nm}}\cos \mathrm{m\λ}+S_{\mathrm{nm}}\sin \mathrm{m\λ}]P_{\mathrm{nm}}\left(\cos \mathrm{\θ}\right),$

Earth mean radius is R, θ=90-L is geocentric colatitude degree, potential coefficient C _nm, S _nmbe the coefficient of known gravity disturbance, obtain by EGM2008 gravity field model, L represents terrestrial latitude, and λ is terrestrial longitude, and ρ is the earth's core radius vector of aiming at place place, and GM is earth constant coefficient, P _nm() is association Legendre polynomial:

$P_{\mathrm{nm}}\left(x\right)={(1-x^2)}^{m/2}\underset{k=0}{\overset{N}{\mathrm{\Σ}}}\frac{{(-1)}^k(2n-2k)!}{2^{n}k!(n-k)!(n-m-2k)!}{x}^{n-m-2k},$

X is variable, | x| < 1; N is called rank, and m is called secondary, N=[(n-m) 2];

The gradient of gravity disturbance position is gravity disturbance vector δ g ⁿ:

$\mathrm{\&delta;}{g}^n=\mathrm{grad}\mathrm{dT}(\mathrm{\&rho;},L,\mathrm{\&lambda;})={(\frac{\&PartialD;T}{\&PartialD;\mathrm{\&rho;}},\frac{\&PartialD;T}{\&PartialD;L},\frac{\&PartialD;T}{\&PartialD;\mathrm{\&lambda;}})}^T,$

The gravity disturbance value under polar coordinate system is obtained by gravity disturbance vector:

$T_{\mathrm{\&rho;}}=\frac{\&PartialD;T}{\&PartialD;\mathrm{\&rho;}}=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}(n+1){\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)$ $T_L=\frac{\&PartialD;T}{\&PartialD;L}=-\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}{\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]\frac{d{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)}{\mathrm{dL}}$

$T_{\mathrm{\&lambda;}}=\frac{\&PartialD;T}{\&PartialD;\mathrm{\&lambda;}}=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}{\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[-{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}]{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)$

-ENU geographic coordinate in sky is fastened northeastward, and the component of gravity disturbance vector 3 axis is being transformed into geographic coordinate is:

${\mathrm{\&delta;g}}_E^n=\frac{T_{\mathrm{\&lambda;}}}{\mathrm{\&rho;}\cos L},{\mathrm{\&delta;g}}_N^n=\frac{T_L}{\mathrm{\&rho;}},\mathrm{\&delta;}{g}_U^n=T_{\mathrm{\&rho;}},$

Then the output of accelerometer is compensated.

Initial matrix:

θ ₀pitch angle, γ ₀roll angle, it is course angle.

State variable is the misaligned angle of the platform, horizontal velocity error.

Step (5) comprising:

(5.1) the Sigma point of 2n+1 dimension is constructed

$\left\{\begin{array}{c}{\mathrm{\&xi;}}_0={\hat{x}}_{k-1}\\ {\mathrm{\&xi;}}_i={\hat{x}}_{k-1}+\sqrt{(m+\mathrm{\&kappa;})P_{k-1}}\\ {\mathrm{\&xi;}}_{i+n}={\hat{x}}_{k-1}-\sqrt{(m+\mathrm{\&kappa;})P_{k-1}}\end{array}\right.,i=\mathrm{1,2},...,m$

Wherein for the state estimation in k-1 moment, P _k-1for the state error covariance matrix in k-1 moment, m is the dimension of system state variables, and κ is scale-up factor, can be used for regulating Sigma point with distance;

(5.2) weights are determined

$w_i^m=w_i^c=\left\{\begin{array}{cc}\mathrm{\&kappa;}/(m+\mathrm{\&kappa;})& (i=0)\\ 1/\left[2(m+\mathrm{\&kappa;})\right]& (i\&NotEqual;0)\end{array}\right.$

(5.3) time renewal is carried out by filtering equations

γ _k/k-1＝f _k-1(ξ _k-1)

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i{\mathrm{\&gamma;}}_{i,k/k-1}$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1}$

By and P _k-1calculate Sigma point ξ _k-1, by nonlinear state function f _k-1() propagates as γ _k/k-1, by γ _k/k-1status predication can be obtained with predicting covariance battle array P _k/k-1, Q _k-1for system noise;

(5.4) carry out measurement by filtering equations to upgrade

K _k＝P _k/k-1H ^T(HP _k/k-1H ^T+R _k) ^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H{\hat{x}}_{k/k-1})$

P _k＝(I-K _kH)P _k/k-1

Wherein R _kfor measurement noise, K _kfor filter gain, P _kfor evaluated error covariance matrix, for state estimation is comprising the misaligned angle of the platform.

Filtering equations comprises state equation

system noise vector $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

Factor arrays $G=\left[\begin{array}{cc}{G}_1& 0_{3\&times;2}\\ 0_{2\&times;3}& G_2\end{array}\right],G_1=\left[\begin{array}{ccc}{G}_{11}^{\&prime;}& G_{12}^{\&prime;}& C_{13}^{\&prime;}\\ G_{21}^{\&prime;}& C_{22}^{\&prime;}& C_{23}^{\&prime;}\\ C_{31}^{\&prime;}& C_{32}^{\&prime;}& C_{33}^{\&prime;}\end{array}\right]G_2=\left[\begin{array}{cc}{C}_{11}^{\&prime;}& C_{12}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}\end{array}\right],$

F (X) is

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_x=-\left(\sin {\mathrm{\&phi;}}_z\right){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&phi;}}_y{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\&delta;}{v}_y}{R_M}+C_{11}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_y=(1-\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-{\mathrm{\&phi;}}_x{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\&delta;}{v}_x}{R_N}+C_{21}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_z=(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\&delta;}{v}_x\tan L}{R_N}+C_{31}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\&phi;}}_z-1)+f_y\sin {\mathrm{\&phi;}}_z-f_z({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}{\mathrm{\&delta;v}}_y\sin L+\\ C_{11}^{\&prime;}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\&prime;}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}_y=-f_x\sin {\mathrm{\&phi;}}_z-f_y(\cos {\mathrm{\&phi;}}_z-1)-f_z({\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z-{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_x\sin L+\\ C_{21}^{\&prime;}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\&prime;}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

Wherein, R _m, R _nbe respectively earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches, φ _x, φ _y, φ _zit is the misaligned angle of the platform in three directions; δ v _x, δ v _yfor velocity error; L is local latitude; w _iefor rotational-angular velocity of the earth; be the gyroscopic drift of three axis; for gyro zero mean Gaussian white noise; the acceleration zero being three axis is inclined; for accelerometer zero mean Gaussian white noise; f _x, f _y, f _zfor acceleration exports the value of specific force on computed geographical coordinates; C ' _ijthat carrier is tied to the element calculated in Department of Geography's matrix;

Measurement equation is:

Z＝HX+V

Wherein, measurement amount Z is inertial navigation horizontal velocity error delta v _x, δ v _y; H=[I _{2 × 2}0 _{2 × 3}], I _{2 × 2}for unit second-order matrix, 0 _{2 × 3}be 2 row 3 row null matrix; V is measurement noise.

Beneficial effect of the present invention is: in the system state space model process that the present invention sets up, misalignment and velocity error model all adopt non-linear form, can describe actual strapdown inertial navitation system (SINS) error Propagation Property so accurately; When traditional inertial navigation resolves usually be normal gravity, and reality is due to earth interior non-uniform mass, and gravitational vector comprises normal gravity and gravity disturbance.Because different regions earth interior mass distribution is different, these gravity disturbances are changes on earth's surface or sea level.Inertial acceleration meter can not distinguish the acceleration of gravity disturbance and body, the existence of gravity disturbance can cause certain initial attitude error, this error can pass through the precision of attitude algorithm, velocity calculated and location compute influential system indices, and this is a disadvantageous factor.Along with inertia device precision improves constantly the development with High Accuracy Inertial Navigation System demand, gravity disturbance becomes a main error source of High Accuracy Inertial initial alignment.In the present invention, in inertial navigation initial alignment, the compensation method of gravity disturbance is, the gravity disturbance value of alignment point is calculated by EGM2008 model according to known latitude and longitude value, then the gravity disturbance value calculated weeds out from accelerometer, just obtain the output that gravity disturbance compensates post-acceleration meter, simulation result shows that gravity disturbance can improve initial alignment precision after compensating.

Accompanying drawing explanation

Fig. 1 represents process flow diagram of the present invention.

Fig. 2 represents misalignment error curve diagram when there is gravity disturbance

Fig. 3 represents and compensates misalignment error curve diagram after gravity disturbance.

Embodiment

Below in conjunction with accompanying drawing, the present invention is described further.

1, the data of fibre optic gyroscope and quartz accelerometer output are gathered;

2, calculate the gravity disturbance value of aiming at place, then the output of accelerometer is compensated.Earth gravity field model is exactly represent earth gravitational field with a gravitation position Spherical harmonic expansion being truncated to N rank, and T (ρ, L, λ) is disturbing potential, and earth mean radius is R, θ=90-L is geocentric colatitude degree.

$T(\mathrm{\&rho;},L,\mathrm{\&lambda;})=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{N}{\mathrm{\&Sigma;}}}{\left(\frac{R}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[C_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+S_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]P_{\mathrm{nm}}\left(\cos \mathrm{\&theta;}\right)---\left(1\right)$

In formula: potential coefficient C _nm, S _nmbe the coefficient of known gravity disturbance, obtain by EGM2008 gravity field model, L represents terrestrial latitude, and λ is terrestrial longitude, and ρ is the earth's core radius vector at calculation level place, and GM is earth constant coefficient, P _nm() is association Legendre polynomial, and definition is as follows:

$P_{\mathrm{nm}}\left(x\right)={(1-x^2)}^{m/2}\underset{k=0}{\overset{N}{\mathrm{\&Sigma;}}}\frac{{(-1)}^k(2n-2k)!}{2^{n}k!(n-k)!(n-m-2k)!}{x}^{n-m-2k}---\left(2\right)$

In formula: in formula: x is variable, | x| < 1; N is called rank, and m is called secondary, N=[(n-m) 2].

In conjunction with association Legendre polynomial, can obtain terrestrial gravitation disturbing potential model T (ρ, L, λ) according to formula (1), the gradient of this model is gravity disturbance vector δ g ⁿ:

$\mathrm{\&delta;}{g}^n=\mathrm{grad}\mathrm{dT}(\mathrm{\&rho;},L,\mathrm{\&lambda;})={(\frac{\&PartialD;T}{\&PartialD;\mathrm{\&rho;}},\frac{\&PartialD;T}{\&PartialD;L},\frac{\&PartialD;T}{\&PartialD;\mathrm{\&lambda;}})}^T---\left(3\right)$

The gravity disturbance value under polar coordinate system is obtained by (3) formula:

$T_{\mathrm{\&rho;}}=\frac{\&PartialD;T}{\&PartialD;\mathrm{\&rho;}}=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}(n+1){\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)$

$T_L=\frac{\&PartialD;T}{\&PartialD;L}=-\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}{\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]\frac{d{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)}{\mathrm{dL}}---\left(4\right)$

$T_{\mathrm{\&lambda;}}=\frac{\&PartialD;T}{\&PartialD;\mathrm{\&lambda;}}=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}{\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[-{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}]{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)$

In east, north, sky geographic coordinate system, the component of gravity disturbance vector 3 axis is because formula (4) is polar coordinate transform representation, be transformed into geographic coordinate system, had following relational expression:

${\mathrm{\&delta;g}}_E^n=\frac{T_{\mathrm{\&lambda;}}}{\mathrm{\&rho;}\cos L},{\mathrm{\&delta;g}}_N^n=\frac{T_L}{\mathrm{\&rho;}},\mathrm{\&delta;}{g}_U^n=T_{\mathrm{\&rho;}}---\left(5\right)$

According to known latitude and longitude value and utilize formula (4), (5) can calculate and aim at the gravity disturbance value in place, then the output of accelerometer is compensated;

3, adopt analytical method to complete coarse alignment, tentatively determine the attitude information of carrier

In formula pitch angle respectively, roll angle, course angle;

4, coarse alignment terminates rear usual horizontal misalignment is little misalignment, and azimuthal misalignment angle is large misalignment angle, therefore sets up Large azimuth angle Nonlinear Filtering Formulae;

5, filtering initialization;

6, utilize UKF filtering method to carry out filtering and estimate misalignment;

7, the strapdown initial matrix of the misaligned angle of the platform update the system estimated is utilized obtain accurate initial strap-down matrix namely thus complete accurate initial alignment.

Embodiment 1

1, the data of fibre optic gyroscope and quartz accelerometer output are gathered;

2, calculate the gravity disturbance value of aiming at place, then the output of accelerometer is compensated.Earth gravity field model is exactly represent earth gravitational field with a gravitation position Spherical harmonic expansion being truncated to N rank, and T (ρ, L, λ) is disturbing potential, and earth mean radius is R, θ=90-L is geocentric colatitude degree.

$T(\mathrm{\&rho;},L,\mathrm{\&lambda;})=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{N}{\mathrm{\&Sigma;}}}{\left(\frac{R}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[C_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+S_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]P_{\mathrm{nm}}\left(\cos \mathrm{\&theta;}\right)---\left(1\right)$

In formula: potential coefficient C _nm, S _nmbe the coefficient of known gravity disturbance, obtain by EGM2008 gravity field model, L represents terrestrial latitude, and λ is terrestrial longitude, and ρ is the earth's core radius vector at calculation level place, and GM is earth constant coefficient, P _nm() is association Legendre polynomial, and definition is as follows:

$P_{\mathrm{nm}}\left(x\right)={(1-x^2)}^{m/2}\underset{k=0}{\overset{N}{\mathrm{\&Sigma;}}}\frac{{(-1)}^k(2n-2k)!}{2^{n}k!(n-k)!(n-m-2k)!}{x}^{n-m-2k}---\left(2\right)$

In formula: in formula: x is variable, | x| < 1; N is called rank, and m is called secondary, N=[(n-m) 2].

In conjunction with association Legendre polynomial, can obtain terrestrial gravitation disturbing potential model T (ρ, L, λ) according to formula (1), the gradient of this model is gravity disturbance vector δ g ⁿ:

$\mathrm{\&delta;}{g}^n=\mathrm{grad}\mathrm{dT}(\mathrm{\&rho;},L,\mathrm{\&lambda;})={(\frac{\&PartialD;T}{\&PartialD;\mathrm{\&rho;}},\frac{\&PartialD;T}{\&PartialD;L},\frac{\&PartialD;T}{\&PartialD;\mathrm{\&lambda;}})}^T---\left(3\right)$

The gravity disturbance value under polar coordinate system is obtained by (3) formula:

$T_{\mathrm{\&rho;}}=\frac{\&PartialD;T}{\&PartialD;\mathrm{\&rho;}}=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}(n+1){\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)$

$T_L=\frac{\&PartialD;T}{\&PartialD;L}=-\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}{\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]\frac{d{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)}{\mathrm{dL}}---\left(4\right)$

$T_{\mathrm{\&lambda;}}=\frac{\&PartialD;T}{\&PartialD;\mathrm{\&lambda;}}=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}{\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[-{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}]{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)$

In east, north, sky geographic coordinate system, the component of gravity disturbance vector 3 axis is because formula (4) is polar coordinate transform representation, be transformed into geographic coordinate system, had following relational expression:

${\mathrm{\&delta;g}}_E^n=\frac{T_{\mathrm{\&lambda;}}}{\mathrm{\&rho;}\cos L},{\mathrm{\&delta;g}}_N^n=\frac{T_L}{\mathrm{\&rho;}},\mathrm{\&delta;}{g}_U^n=T_{\mathrm{\&rho;}}---\left(5\right)$

According to known latitude and longitude value and utilize formula (4), (5) can calculate and aim at the gravity disturbance value in place, then the output of accelerometer is compensated;

3, adopt analytical method to carry out the coarse alignment of completion system, tentatively determine the attitude information of carrier

In formula pitch angle respectively, roll angle, course angle;

4, coarse alignment terminates rear usual horizontal misalignment is little misalignment, and azimuthal misalignment angle is large misalignment angle, therefore sets up strapdown inertial navigation system initial alignment nonlinear state equation system noise vector $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

Factor arrays $G=\left[\begin{array}{cc}{G}_1& 0_{3\&times;2}\\ 0_{2\&times;3}& G_2\end{array}\right],G_1=\left[\begin{array}{ccc}{G}_{11}^{\&prime;}& G_{12}^{\&prime;}& C_{13}^{\&prime;}\\ G_{21}^{\&prime;}& C_{22}^{\&prime;}& C_{23}^{\&prime;}\\ C_{31}^{\&prime;}& C_{32}^{\&prime;}& C_{33}^{\&prime;}\end{array}\right]G_2=\left[\begin{array}{cc}{C}_{11}^{\&prime;}& C_{12}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}\end{array}\right],$ F (X) is

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_x=-\left(\sin {\mathrm{\&phi;}}_z\right){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&phi;}}_y{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\&delta;}{v}_y}{R_M}+C_{11}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_y=(1-\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-{\mathrm{\&phi;}}_x{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\&delta;}{v}_x}{R_N}+C_{21}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_z=(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\&delta;}{v}_x\tan L}{R_N}+C_{31}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\&phi;}}_z-1)+f_y\sin {\mathrm{\&phi;}}_z-f_z({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}{\mathrm{\&delta;v}}_y\sin L+\\ C_{11}^{\&prime;}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\&prime;}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}_y=-f_x\sin {\mathrm{\&phi;}}_z-f_y(\cos {\mathrm{\&phi;}}_z-1)-f_z({\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z-{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_x\sin L+\\ C_{21}^{\&prime;}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\&prime;}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

Wherein, R _m, R _nbe respectively earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches, φ _x, φ _y, φ _zit is the misalignment (quantity of state) in three directions; δ v _x, δ v _yfor velocity error (quantity of state); L is local latitude; w _iefor rotational-angular velocity of the earth; be the gyroscopic drift of three axis; for gyro zero mean Gaussian white noise; the acceleration zero being three axis is inclined; for accelerometer zero mean Gaussian white noise; f _x, f _y, f _zfor acceleration exports the value of specific force on computed geographical coordinates; C ' _ijthat carrier is tied to the element calculated in Department of Geography's matrix;

Initial alignment measurement equation is:

Z＝HX+V

Wherein, measurement amount Z is inertial navigation horizontal velocity error delta v _x, δ v _y; H=[I _{2 × 2}0 _{2 × 3}] (I _{2 × 2}for unit two bit matrix, 0 _{2 × 3}be 2 row 3 row null matrix); V is measurement noise;

5, filtering initial value is chosen

${\hat{x}}_0=E\left[x_0\right],P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]$

Wherein x ₀for the initial value of state variable, P ₀for the initial error covariance matrix of state variable.

6, utilize UKF method to carry out filtering and estimate misalignment, specific as follows;

(1) the Sigma point of 2n+1 dimension is constructed

$\left\{\begin{array}{c}{\mathrm{\&xi;}}_0={\hat{x}}_{k-1}\\ {\mathrm{\&xi;}}_i={\hat{x}}_{k-1}+\sqrt{(m+\mathrm{\&kappa;})P_{k-1}},\\ {\mathrm{\&xi;}}_{i+n}={\hat{x}}_{k-1}-\sqrt{(m+\mathrm{\&kappa;})P_{k-1}}\end{array}\right.,i=\mathrm{1,2},...,n$

Wherein for the state estimation in k-1 moment, P _k-1for the state error covariance matrix in k-1 moment, n is the dimension of system state variables, and κ is scale-up factor, can be used for regulating Sigma point with distance.

(2) weights are determined

$w_i^m=w_i^c=\left\{\begin{array}{cc}\mathrm{\&kappa;}/(m+\mathrm{\&kappa;})& (i=0)\\ 1/\left[2(m+\mathrm{\&kappa;})\right]& (i\&NotEqual;0)\end{array}\right.$

(3) time upgrades

γ _k/k-1＝f _k-1(ξ _k-1)

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i{\mathrm{\&gamma;}}_{i,k/k-1}$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1}$

According to the Sigma sampling policy selected by (2) step, by and P _k-1calculate Sigma point ξ _k-1, by nonlinear state function f _k-1() propagates as γ _k/k-1, by γ _k/k-1status predication can be obtained with predicting covariance battle array P _k/k-1, Q _k-1for system noise.

(4) renewal is measured

K _k＝P _k/k-1H ^T(HP _k/k-1H ^T+R _k) ^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H{\hat{x}}_{k/k-1})$

P _k＝(I-K _kH)P _k/k-1

Wherein R _kfor measurement noise, K _kfor filter gain, P _kfor evaluated error covariance matrix

7, the strapdown initial matrix of the misaligned angle of the platform update the system estimated is utilized obtain accurate initial strap-down matrix namely thus complete accurate initial alignment.

8, matlab simulating, verifying is carried out to the method in the present invention:

Carrier initial position: north latitude 45.7996 °, east longitude 126.6705 °, earth radius R=6378393m, gyroscope constant value drift 0.001 °/h, random walk coefficient is 0.0002 °/h; Initial misalignment φ _x=1 °, φ _y=1 °, φ _z=10 °, accelerometer bias 10 μ g, zero inclined white noise is 5 μ g, it is 0.01s that inertial sensor data produces the cycle, utilize the gravity field model of the 2.5' × 2.5' resolution provided to simulate the A/W of alignment point in emulation, and the gravity field model utilizing resolution to be 5' × 5' calculating compensate gravity disturbance.According to set parameter, carry out initial alignment emulation when first there is gravity disturbance and obtain Fig. 2 misalignment graph of errors, then utilize gravity disturbance compensation method of the present invention can obtain the east orientation misalignment error delta φ shown in Fig. 3 _e, north orientation misalignment error delta φ _n, sky is to misalignment error delta φ _ucurve.Can find out that gravity disturbance can improve initial alignment precision after compensating from the contrast of Fig. 2 and Fig. 3.

Claims (6)

1. a compensation method for gravity disturbance in inertial navigation initial alignment, is characterized in that:

(1) angular velocity data of fibre optic gyroscope output and the acceleration information of quartz accelerometer output is gathered;

(2) calculated the gravity disturbance value of aiming at place by gravity disturbance position, the output acceleration information of quartz accelerometer is compensated;

(3) adopt analytical method to complete coarse alignment, pass through initial matrix determine attitude information and pitch angle, roll angle, the course angle of carrier, wherein b represents carrier coordinate system, and n ' representative calculates navigational coordinate system;

(4) filtering initial value is chosen

${\hat{x}}_0=E\left[x_0\right],P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right].$

Wherein x ₀for the initial value of state variable, P ₀for the initial error covariance matrix of state variable;

(5) utilize Unscented kalman filtering method to carry out filtering, estimate the misaligned angle of the platform;

(6) the strapdown initial matrix of the misaligned angle of the platform update the system estimated is utilized obtain accurate initial strap-down matrix namely thus complete accurate initial alignment.

2. the compensation method of gravity disturbance in a kind of inertial navigation initial alignment according to claim 1, is characterized in that: described gravity disturbance position T (ρ, L, λ),

$T(\mathrm{\&rho;},L,\mathrm{\&lambda;})=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{N}{\mathrm{\&Sigma;}}}{\left(\frac{R}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[C_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+S_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]P_{\mathrm{nm}}\left(\cos \mathrm{\&theta;}\right),$

Earth mean radius is R, θ=90-L is geocentric colatitude degree, potential coefficient C _nm, S _nmbe the coefficient of known gravity disturbance, obtain by EGM2008 gravity field model, L represents terrestrial latitude, and λ is terrestrial longitude, and ρ is the earth's core radius vector of aiming at place place, and GM is earth constant coefficient, P _nm() is association Legendre polynomial:

$P_{\mathrm{nm}}\left(x\right)={(1-x^2)}^{m/2}\underset{k=0}{\overset{N}{\mathrm{\&Sigma;}}}\frac{{(-1)}^k(2n-2k)!}{2^{n}k!(n-k)!(n-m-2k)!}{x}^{n-m-2k},$

X is variable, | x| < 1; N is called rank, and m is called secondary, N=[(n-m)/2];

The gradient of gravity disturbance position is gravity disturbance vector δ g ⁿ:

$\mathrm{\&delta;}{g}^n=\mathrm{grad}\mathrm{dT}(\mathrm{\&rho;},L,\mathrm{\&lambda;})={(\frac{\&PartialD;T}{\&PartialD;\mathrm{\&rho;}},\frac{\&PartialD;T}{\&PartialD;L},\frac{\&PartialD;T}{\&PartialD;\mathrm{\&lambda;}})}^T,$

The gravity disturbance value under polar coordinate system is obtained by gravity disturbance vector:

$T_{\mathrm{\&rho;}}=\frac{\&PartialD;T}{\&PartialD;\mathrm{\&rho;}}=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}(n+1){\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)$

$T_L=\frac{\&PartialD;T}{\&PartialD;L}=-\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}{\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}]\frac{d{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)}{\mathrm{dL}}$

$T_{\mathrm{\&lambda;}}=\frac{\&PartialD;T}{\&PartialD;\mathrm{\&lambda;}}=\frac{\mathrm{GM}}{\mathrm{\&rho;}}\underset{n=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}{\left(\frac{a}{\mathrm{\&rho;}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\&Sigma;}}}[-{\stackrel{\&OverBar;}{C}}_{\mathrm{nm}}\sin \mathrm{m\&lambda;}+{\stackrel{\&OverBar;}{S}}_{\mathrm{nm}}\cos \mathrm{m\&lambda;}]{\stackrel{\&OverBar;}{P}}_{\mathrm{nm}}\left(\sin L\right)$

-ENU geographic coordinate in sky is fastened northeastward, and the component of gravity disturbance vector 3 axis is being transformed into geographic coordinate is:

$\mathrm{\&delta;}{g}_E^n=\frac{T_{\mathrm{\&lambda;}}}{\mathrm{\&rho;}\cos L},\mathrm{\&delta;}{g}_N^n=\frac{T_L}{\mathrm{\&rho;}},\mathrm{\&delta;}{g}_U^n=T_{\mathrm{\&rho;}},$

Then the output of accelerometer is compensated.

3. the compensation method of gravity disturbance in a kind of inertial navigation initial alignment according to claim 1, is characterized in that: described initial matrix:

θ ₀pitch angle, γ ₀roll angle, it is course angle.

4. the compensation method of gravity disturbance in a kind of inertial navigation initial alignment according to claim 1, is characterized in that: described state variable is the misaligned angle of the platform, horizontal velocity error.

5. the compensation method of gravity disturbance in a kind of inertial navigation initial alignment according to claim 1, is characterized in that: described step (5) comprising:

(5.1) the Sigma point of 2n+1 dimension is constructed

$\left\{\begin{array}{c}{\mathrm{\&xi;}}_0={\hat{x}}_{k-1}\\ {\mathrm{\&xi;}}_i={\hat{x}}_{k-1}+\sqrt{(m+\mathrm{\&kappa;})P_{k-1}},i=\mathrm{1,2},...,m\\ {\mathrm{\&xi;}}_{i+n}={\hat{x}}_{k-1}-\sqrt{(m+\mathrm{\&kappa;})P_{k-1}}\end{array}\right.$

Wherein for the state estimation in k-1 moment, P _k-1for the state error covariance matrix in k-1 moment, m is the dimension of system state variables, and κ is scale-up factor, can be used for regulating Sigma point with distance;

(5.2) weights are determined

$w_i^m=w_i=\left\{\begin{array}{cc}\mathrm{\&kappa;}/(m+\mathrm{\&kappa;})& (i=0)\\ 1/\left[2(m+\mathrm{\&kappa;})\right]& (i\&NotEqual;0)\end{array}\right.$

(5.3) time renewal is carried out by filtering equations

γ _k/k-1＝f _k-1(ξ _k-1)

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i{\mathrm{\&gamma;}}_{i,k/k-1}$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1}$

By and P _k-1calculate Sigma point ξ _k-1, by nonlinear state function f _k-1() propagates as γ _k/k-1, by γ _k/k-1status predication can be obtained with predicting covariance battle array P _k/k-1, Q _k-1for system noise;

(5.4) carry out measurement by filtering equations to upgrade

K _k＝P _k/k-1H ^T(HP _k/k-1H ^T+R _k) ^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H{\hat{x}}_{k/k-1})$

P _k＝(I-K _kH)P _k/k-1

Wherein R _kfor measurement noise, K _kfor filter gain, P _kfor evaluated error covariance matrix, for state estimation is comprising the misaligned angle of the platform.

6. the compensation method of gravity disturbance in a kind of inertial navigation initial alignment according to claim 5, is characterized in that: described filtering equations comprises state equation

$\stackrel{.}{X=}f\left(X\right)+\mathrm{GW},$ System noise vector $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

Factor arrays $G=\left[\begin{array}{cc}{G}_1& 0_{3\&times;2}\\ 0_{2\&times;3}& G_2\end{array}\right],G_1=\left[\begin{array}{ccc}{C}_{11}^{\&prime;}& C_{12}^{\&prime;}& C_{13}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}& C_{23}^{\&prime;}\\ C_{31}^{\&prime;}& C_{32}^{\&prime;}& C_{33}^{\&prime;}\end{array}\right]G_2=\left[\begin{array}{cc}{G}_{11}^{\&prime;}& G_{12}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}\end{array}\right],$

F (X) is

${\stackrel{.}{\mathrm{\&phi;}}}_x=-\left(\sin {\mathrm{\&phi;}}_z\right){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&phi;}}_y{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\&delta;}{v}_y}{R_M}+C_{11}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\&phi;}}}_y=(1-\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&phi;}}_x{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\&delta;}{v}_x}{R_N}+C_{21}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\&phi;}}}_z=(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\&delta;}{v}_x\tan L}{R_N}+C_{31}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\&phi;}}_z-1)+f_y\sin {\mathrm{\&phi;}}_z-f_z({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_y\sin L+\\ C_{11}^{\&prime;}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\&prime;}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_y=-f_x\sin {\mathrm{\&phi;}}_z-f_y(\cos {\mathrm{\&phi;}}_z-1)f_z({\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z-{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_x\sin L+\\ C_{21}^{\&prime;}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\&prime;}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

Wherein, R _m, R _nbe respectively earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches, φ _x, φ _y, φ _zit is the misaligned angle of the platform in three directions; δ v _x, δ v _yfor velocity error; L is local latitude; w _iefor rotational-angular velocity of the earth; be the gyroscopic drift of three axis; for gyro zero mean Gaussian white noise; the acceleration zero being three axis is inclined; for accelerometer zero mean Gaussian white noise; f _x, f _y, f _zfor acceleration exports the value of specific force on computed geographical coordinates; C ' _ijthat carrier is tied to the element calculated in Department of Geography's matrix;

Measurement equation is:

Z＝HX+V

Wherein, measurement amount Z is inertial navigation horizontal velocity error delta v _x, δ v _y; H=[I _{2 × 2}0 _{2 × 3}], I _{2 × 2}for unit second-order matrix, 0 _{2 × 3}be 2 row 3 row null matrix; V is measurement noise.