CN105737823A - GPS/SINS/CNS integrated navigation method based on five-order CKF - Google Patents

Abstract

The invention discloses a GPS/SINS/CNS integrated navigation method based on a five-order cubature kalman filter (CKF).The method is characterized by comprising the steps of 1, establishing an integrated navigation system non-linear error equation and a linear measurement equation based on a SINS error equation; 2, establishing the five-order CKF by means of the five-order spherical surface radial cubature rule; 3, filtering and fusing information output by a SINS, a GPS and a CNS by means of the five-order CKF to obtain the optimal estimation of navigation parameters.

Description

GPS/SINS/CNS Combinated navigation method based on five rank CKF

Technical field

The present invention relates to strap-down inertial and integrated navigation technology field, especially a kind of SINS/GPS/CNS Combinated navigation method based on five rank CKF.

Background technology

In SINS/GPS/CNS integrated navigation system, GPS can provide accurate velocity information at any time, and therefore the velocity error of inertial navigation can be controlled the accuracy rating at GPS by combined system.CNS system can provide accurate attitude angle information, can improve the certainty of measurement of attitude angle after combined, but due to the restriction of objective condition, can only be interrupted offer course information, and this just requires that the estimation of course error must be stablized without in the interval of course information.For these reasons, it is necessary in integrated navigation system, introduce a kind of more advanced filtering algorithm, overcome system model parameter inaccurate or with the impact of environmental change, to improve the system model adaptability for external environment condition, and then provide high-precision navigation information.

Therefore, study CKF filtering method, improve its filtering performance further, and be applied to the Nonlinear Filtering Problem in SINS/GPS/CNS integrated navigation system, no matter be in theory, or in engineering practice, be respectively provided with significance.

Summary of the invention

Goal of the invention: for overcoming the deficiencies in the prior art, the invention provides a kind of GPS/SINS/CNS Combinated navigation method based on 5 rank CKF improving filtering accuracy and filtration efficiency.

Technical scheme: for solving above-mentioned technical problem, the invention provides the GPS/SINS/CNS Combinated navigation method based on five rank CKF, it is characterised in that comprise the steps:

Step 1: set up the integrated navigation system nonlinearity erron equation based on SINS error equation and linear measurement equation；

Step 2: utilize five rank spheres radially volume rule criteria construction five rank volume Kalman filter；

Step 3: utilize the five rank CKF information that SINS and GPS, CNS are exported to be filtered, merge, obtain optimal estimation the erection rate information of navigational parameter, positional information and strapdown attitude matrix, to obtain accurate navigation information.

Further, described step 1 set up the integrated navigation system nonlinearity erron equation based on SINS error equation and linear measurement equation particularly as follows:

Step 1.1: choosing " sky, northeast " geographic coordinate system is navigational coordinate system n system；Choosing carrier " on before the right side " coordinate system is carrier coordinate system b system；N system successively turns to b system through 3 Eulerian angles, three Eulerian angles be designated as course angle ψ ∈ (-π, π], pitch angleRoll angle γ ∈ (-π, π]；Attitude matrix between n system and b system is designated asTrue attitude angle is designated asTrue velocity is designated as $v_s^n={\left[\begin{array}{ccc}{v}_e^n& v_n^n& v_u^n\end{array}\right]}^T;$ True geographic coordinate system is P=[L λ H]^T, wherein, L is latitude, and λ is longitude, and H is height；The attitude angle that SINS calculates is designated asSpeed is designated as ${\tilde{v}}_s^n={\left[\begin{array}{ccc}{\tilde{v}}_e^n& {\tilde{v}}_n^n& {\tilde{v}}_u^n\end{array}\right]}^T,$ Geographic coordinate system is $\tilde{P}={\left[\begin{array}{ccc}\tilde{L}& \widetilde{\mathrm{\λ}}& \tilde{H}\end{array}\right]}^T;$ The mathematical platform that SINS calculates is designated as n ', and the attitude matrix that n ' is between b system is designated asNote attitude error isVelocity error is: ${\mathrm{\δv}}^n=v_s^n-{\tilde{v}}_s^n={\left[\begin{array}{ccc}{\mathrm{\δv}}_e^n& {\mathrm{\δv}}_n^n& {\mathrm{\δv}}_u^n\end{array}\right]}^T,$ Site error is: $\mathrm{\δ}P=P-\tilde{P}=\[\begin{array}{ccc}\mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}H\end{array}\].$ Then Nonlinear Error Models is as follows:

$\stackrel{\·}{\mathrm{\φ}}=C_{\mathrm{\ω}}^{-1}\[(I-C_n^{n^{\′}}){\widetilde{\mathrm{\ω}}}_{in}^n+C_n^{n^{\′}}{\mathrm{\δ\ω}}_{in}^b-C_b^{n^{\′}}({\mathrm{\ϵ}}^b+w_g^b)\]$

$\mathrm{\δ}{\stackrel{\·}{v}}^n=\[(I-C_{n^{\′}}^n)\]C_b^{n^{\′}}{\tilde{f}}^b-(2{\mathrm{\δ\ω}}_{ie}^n+{\mathrm{\δ\ω}}_{en}^n)\×{\tilde{v}}^n-(2{\widetilde{\mathrm{\ω}}}_{ie}^n+{\widetilde{\mathrm{\ω}}}_{en}^n)\×{\mathrm{\δv}}^n+C_{n^{\′}}^n{C}_b^{n^{\′}}({\▿}^b+w_a^b)$

$\mathrm{\δ}\stackrel{\·}{L}=\frac{v_n^n}{R_M+H}-\frac{{\tilde{v}}_n^n}{R_M+H}=\frac{{\mathrm{\δv}}_n^n}{R_M+H}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{v_E^n\sec L}{R_N+H}-\frac{{\tilde{v}}_E^n\sec \tilde{L}}{R_N+H}$

Wherein, ${\mathrm{\ϵ}}^b=\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]$ The constant error of three axle gyros is descended for b system,Random error for the lower three axle gyros of b system is zero-mean white-noise process, ${\▿}^b={\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]}^T$ 3-axis acceleration constant error is descended for b system,Random error for the lower 3-axis acceleration of n system is zero-mean white-noise process,For the actual output of accelerometer,It is mathematical platform angular velocity of rotation under b system,For the mathematical platform angular velocity of rotation calculated,For earth rotation angular velocity,For angular velocity of rotation relative to the earth of the mathematical platform that calculates,For correspondenceCalculating error.R_M、R_NRespectively local west, meridian circle fourth of the twelve Earthly Branches circle radius of curvature,Inverse matrix for Eulerian angles differential equation coefficient matrix；It is the attitude matrix between n system for n ', C_ωWithParticularly as follows:

$C_n^{n^{\′}}=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_n{\mathrm{cos\φ}}_u\_{\mathrm{sin\φ}}_n{\mathrm{sin\φ}}_e{\mathrm{sin\φ}}_u& {\mathrm{cos\φ}}_n{\mathrm{sin\φ}}_u+{\mathrm{sin\φ}}_n{\mathrm{sin\φ}}_e{\mathrm{sin\φ}}_u& \_{\mathrm{sin\φ}}_n{\mathrm{cos\φ}}_e\\ \_{\mathrm{cos\φ}}_e{\mathrm{sin\φ}}_u& {\mathrm{cos\φ}}_e{\mathrm{cos\φ}}_u& {\mathrm{sin\φ}}_e\\ {\mathrm{sin\φ}}_n{\mathrm{cos\φ}}_u+{\mathrm{cos\φ}}_n{\mathrm{sin\φ}}_e{\mathrm{sin\φ}}_u& {\mathrm{sin\φ}}_n{\mathrm{sin\φ}}_u-{\mathrm{cos\φ}}_n{\mathrm{sin\φ}}_e{\mathrm{sin\φ}}_u& {\mathrm{cos\φ}}_n{\mathrm{cos\φ}}_e\end{array}\right]$

$C_{\mathrm{\ω}}=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_n& 0& -{\mathrm{sin\φ}}_n{\mathrm{cos\φ}}_e\\ 0& 1& {\mathrm{sin\φ}}_e\\ {\mathrm{sin\φ}}_n& 0& {\mathrm{cos\φ}}_n{\mathrm{cos\φ}}_e\end{array}\right];$

Step 1.2: it is as follows that described nonlinearity erron equation and linear measurement equation set up process:

Access speed errorEulerian angles platform error angle φ_e、φ_n、φ_u；Site error δ L, δ λ；Accelerometer constant error under b systemAnd gyroscope constant value errorAnd constitute 12 dimension state vectors,H is approximately zero:

$x=\left[\begin{array}{cccccccccccc}{\mathrm{\δv}}_e^n& {\mathrm{\δv}}_n^n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& \mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& {\▿}_x^b& {\▿}_y^b& {\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]$

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=C_{\mathrm{\ω}}^{-1}\[\left(I-C_n^{n^{\′}}\right){\widetilde{\mathrm{\ω}}}_{in}^n+{\mathrm{\δ\ω}}_{in}^n-C_b^{n^{\′}}{\mathrm{\ϵ}}^b\]+w_g^n\\ \mathrm{\δ}{\stackrel{\·}{v}}^n=\left(I-C_{n^{\′}}^n\right)C_b^{n^{\′}}{\tilde{f}}^b-\left(2{\widetilde{\mathrm{\ω}}}_{ie}^n+{\widetilde{\mathrm{\ω}}}_{en}^n\right)\×\mathrm{\δ}{\tilde{v}}^n+C_{n^{\′}}^n{C}_b^{n^{\′}}{\▿}^b+w_a^n\\ \mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_n^n}{R_M}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{v_E^n\sec L}{R_N}-\frac{{\tilde{v}}_E^n\sec \tilde{L}}{R_N}\\ {\stackrel{\·}{\▿}}^b=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}^b=0\end{array}\right.$

Front bidimensional state is only taken with P,WithFor zero-mean white-noise process；.This nonlinear state equation can be abbreviated as:Using sampling period T as filtering the cycle, it is possible to use quadravalence Runge-Kutta integration method, with T for step-length by its discretization, specifically comprise the following steps that

Assume the state vector the chosen value x at initial time₀It is known that and note t₁=t+T/2, then can be incited somebody to action by following iterative formulaDiscrete:

Δx₁=[f (x, t)+ω (t)] T

Δx₂=[f (x, t₁)+Δx₁/2+ω(t₁)]T

Δx₃=[f (x, t₁)+Δx₂/2+ω(t₁)]T

Δx₄=[f (x, t+T)+Δ x₃+ω(t+T)]T

x_k+1=x_k+(Δx₁+2Δx₂+2Δx₃+Δx₄)/6

Remember that discrete rear state filtering equation is: x_k=f (x_k-1)+ω_k-1

It is as follows that linear measurement equation sets up process:

Choose GPS velocity errorM_e、M_nThe respectively range rate error item of GPS component on east orientation north orientation coordinate axes；

Velocity measurement amount vector definition is as follows:

$Z_v\left(t\right)=\left[\begin{array}{c}\mathrm{\δ}{v}_e+M_e\\ {\mathrm{\δv}}_n+M_n\end{array}\right]=H_v\left(t\right)X\left(t\right)+V_v\left(t\right)$

In formula, H_v(t)=[diag [1,1] 0_2×10]

Under strapdown mode of operation, CNS the attitude information exported can obtain the three-axis attitude information of carrierAnd the three-axis attitude information that SINS also can provide carrier by inertial reference calculation isTherefore both are subtracted each other the three-axis attitude error angle Δ φ that can obtain carrier:

$\mathrm{\Δ}\mathrm{\φ}=\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\φ}}_e\\ {\mathrm{\Δ\φ}}_n\\ {\mathrm{\Δ\φ}}_u\end{array}\right]=\left[\begin{array}{c}{\mathrm{\θ}}_0-\widetilde{\mathrm{\θ}}\\ {\mathrm{\γ}}_0-\widetilde{\mathrm{\γ}}\\ {\mathrm{\ψ}}_0-\widetilde{\mathrm{\ψ}}\end{array}\right]$

Δ φ '=M × Δ φ

In formula, M is attitude error angle transition matrix:

$M=\left[\begin{array}{ccc}0& {\mathrm{cos\θ}}_0& -{\mathrm{cos\γ}}_0{\mathrm{sin\θ}}_0\\ 0& {\mathrm{sin\θ}}_0& {\mathrm{cos\γ}}_0{\mathrm{sin\θ}}_0\\ 1& 0& {\mathrm{sin\γ}}_0\end{array}\right]$

Δ φ ' is set up as observation the measurement model of system by integrated navigation system, estimates the error of inertia device in navigation system in real time by the method for optimal estimation, and with this, SINS is modified；

Measurement equation is as follows:

$Z_{\mathrm{\φ}}\left(t\right)=\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\θ}}^{\′}\\ \mathrm{\Δ}{\mathrm{\γ}}^{\′}\\ \mathrm{\Δ}{\mathrm{\ψ}}^{\′}\end{array}\right]=H_{\mathrm{\φ}}X\left(t\right)+V_{\mathrm{\φ}}\left(t\right)$

In formula, H_φ=[0_3×2I_3×30_3×7]

By analyzing above it can be seen that the measurement equation of full combination is:

$Z\left(t\right)=\left[\begin{array}{c}{H}_{\mathrm{\φ}}\left(t\right)\\ H_v\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_{\mathrm{\φ}}\left(t\right)\\ V_v\left(t\right)\end{array}\right]=H\left(t\right)X\left(t\right)+V\left(t\right)$

Same using T as the filtering cycle, and carry out simple discretization using T as step-length.

By state equation and measurement equation filtering equations composed as follows:

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+{\mathrm{\ω}}_{k-1}\\ z_k=h\left(k\right)x_k+v_k\end{array}\right..$

Further, described step 2) utilize five rank spheres radially volume rule criteria construction five rank volume Kalman filter particularly as follows:

Step 2.1: radially volume criterion is as follows for five rank spheres:

$I\left(f\right)={\∫}_{R^n}f\left(x\right)\exp (-x^T{x}^T)dx$

F (x) is arbitrary function, RⁿFor integral domain, the method that use is approximate obtains the approximate solution of its analytic value.

Take x=ry (y^TY=1, r ∈ [0, ∞)), therefore above-mentioned integration can be write as in sphere radial coordinate system:

$I\left(f\right)=\underset{0}{\overset{\mathrm{\∞}}{\∫}}\underset{U_n}{\∫}f\left(ry\right)r^{n-1}{e}^{-r^2}d\mathrm{\σ}\left(y\right)dr$

U_nFor n N-dimension unit sphere, σ () is U_nOn element.Integration I (f) is separated into Spherical integration and Radial amasss:

$S\left(r\right)={\∫}_{U_n}f\left(ry\right)d\mathrm{\σ}\left(y\right);R={\∫}_0^{\mathrm{\∞}}S\left(r\right)r^{n-1}{e}^{-r^2}dr$

Assume the N of Spherica integration S (r)_sSampling approaches:

$S\left(r\right)\≈\underset{j=1}{\overset{N_s}{\Σ}}{\mathrm{\ω}}_{r,j}f\left({\mathrm{ry}}_j\right)$

The N of Radial integration R_rSampling approaches:

$R=\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_{r,i}S\left(r_i\right)$

Obtain the N of I (f)_r×N_sSampling approach into:

$I\left(f\right)\≈\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_{r,i}{\mathrm{\ω}}_{s,j}f\left(r_i{y}_j\right)$

The computing formula of Spherical criterion: S (r) is as follows:

$S\left(r\right)={\mathrm{\ω}}_1\underset{i=1}{\overset{2n}{\Σ}}f\left(r{\[e\]}_i\right)+{\mathrm{\ω}}_2\underset{i=1}{\overset{2n(n-1)}{\Σ}}f\left(r{\[s\]}_i\right)$

Wherein[s]_nFor [e]_nPoint Set, be designated as It is respectively as follows:

${\left\{e_i\right\}}_{i-1}^n={\{{\[1,0,...,0\]}^T,{\[0,1,...,0\]}^T,...,{\[0,0,...,1\]}^T\}}^T$

${\left\{s_i\right\}}_{i=1}^{n(n-1)}=\{(e_k+e_l)/\sqrt{2}:k,l=1,2,...,n;k<l\},$

[e]_n[s]_nInterior vector number respectively 2n and 2n (n-1), [e]_i[s]_iRespectively [e]_n[s]_nI-th row；

$f=1\&DoubleRightArrow;A_n=2{\mathrm{n\&omega;}}_1+2n(n-1){\mathrm{\&omega;}}_2$

$f=y_1^2\&DoubleRightArrow;A_n/n=2{\mathrm{\&omega;}}_1+2(n-1){\mathrm{\&omega;}}_2$

$f=y_1^2{y}_2^2\&DoubleRightArrow;A_n/\&lsqb;n\left(n+2\right)\&rsqb;={\mathrm{\&omega;}}_2$

$f=y_1^4\&DoubleRightArrow;3A_n/\&lsqb;n(n+2)\&rsqb;=2{\mathrm{\&omega;}}_1+(n-1){\mathrm{\&omega;}}_2$

Solve: ${\mathrm{\&omega;}}_1=\frac{(4-n)A_n}{2n(n+2)};{\mathrm{\&omega;}}_2=\frac{A_n}{n(n+2)}$

Radial rule:

$S\left(r\right)=1\&DoubleRightArrow;{\mathrm{\&omega;}}_{r,1}{r}_1^0+{\mathrm{\&omega;}}_{r,2}{r}_2^0=\mathrm{\&Gamma;}\left(n/2\right)/2$

$S\left(r\right)=r^2\&DoubleRightArrow;{\mathrm{\&omega;}}_{r,1}{r}_1^2+{\mathrm{\&omega;}}_{r,2}{r}_2^2=\mathrm{\&Gamma;}\left(n/2+1\right)/2$

$S\left(r\right)=r^4\&DoubleRightArrow;{\mathrm{\&omega;}}_{r,1}{r}_1^4+{\mathrm{\&omega;}}_{r,2}{r}_2^4=\mathrm{\&Gamma;}\left(n/2+2\right)/2$

Utilize general Gauss-Laguerre Integral Rule to solve equation group

${\mathrm{\&omega;}}_{r,1}=\mathrm{\&Gamma;}\left(\frac{n}{2}\right)\frac{\sqrt{\mathrm{\&alpha;}}+1}{4\sqrt{\mathrm{\&alpha;}}};{\mathrm{\&omega;}}_{r,2}=\mathrm{\&Gamma;}\left(\frac{n}{2}\right)\frac{\sqrt{\mathrm{\&alpha;}}-1}{4\sqrt{\mathrm{\&alpha;}}};\mathrm{\&alpha;}=\frac{n}{2}+1$

Step 2.2: the construction method of five rank volume Kalman filter is as follows:

Filtering equations for setting up:

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+{\mathrm{\&omega;}}_{k-1}\\ z_k=h\left(k\right)x_k+v_k\end{array}\right.$

X in formula_kIt is 12 dimension state vectors, and each component is independent, Gaussian distributed；z_kIt is 5 dimension observation vectors；ω_k-1And ν_kRespectively process noise and measurement noise, meets:

$\left\{\begin{array}{cc}E\&lsqb;{\mathrm{\&omega;}}_k\&rsqb;=0,& E\&lsqb;{\mathrm{\&omega;}}_i{{\mathrm{\&omega;}}_j}^T\&rsqb;=Q_k{\mathrm{\&delta;}}_{ij}\\ E\&lsqb;v_k\&rsqb;=0,& E\&lsqb;v_i{v_j}^T\&rsqb;=R_k{\mathrm{\&delta;}}_{ij}\\ E\&lsqb;{\mathrm{\&omega;}}_i{v_j}^T\&rsqb;=0& \end{array}\right.$

In formula, δ_ijFor delta-function；Q is the variance matrix of system noise；R is the variance matrix of measurement noise.

Five rank CKF algorithms are as follows:

Step 2.2.1: the time updates:

5. the Posterior probability distribution in k-1 moment is assumedIt is known that Cholesky decomposes:

P_k-1=S_k-1S_k-1 ^T

6. volume point is calculated:

$x_{1,i}=\stackrel{\&OverBar;}{r_1}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1},x_{2,i}=\stackrel{\&OverBar;}{r_2}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1}$

$x_{3,i}=\stackrel{\&OverBar;}{r_1}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1},x_{4,i}=\stackrel{\&OverBar;}{r_2}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1}$

In formula, i=1,2 ..., 2n；J=1,2 ..., 2n (n-1)

7. volume point is propagated by state equation:

x_1,i ^*=f (x_1,i)；x_2,i ^*=f (x_2,i)

x_3,j ^*=f (x_3,j)；x_4,j ^*=f (x_4,j)

8. the predictive value of k moment state and error battle array is estimated:

${\hat{x}}_{k/k-1}=K\underset{i=1}{\overset{2n}{\&Sigma;}}(k_1{x^{*}}_{1,i}+k_2{x^{*}}_{2,i})+\underset{j=1}{\overset{2n(n-1)}{\&Sigma;}}(k_1{x^{*}}_{3,j}+k_2{x^{*}}_{4,j})$

$P_{xx}=K\underset{i=1}{\overset{2n}{\&Sigma;}}(k_1{x}_{1,i}^{*}{x_{1,i}^{*}}^T+k_2{x}_{2,i}^{*}{x_{2,i}^{*}}^T)-{\hat{x}}_{k/k-1}{\hat{x}}_{k/k-1}^T+\underset{i=1}{\overset{2n(n-1)}{\&Sigma;}}(k_1{x}_{3,j}^{*}{x_{3,j}^{*}}^T+k_2{x}_{4,j}^{*}{x_{4,j}^{*}}^T)+Q_{k-1}$

$K=3-\mathrm{\&alpha;};k_1={\left(8{\mathrm{\&alpha;}}^{\frac{3}{2}}\right({\mathrm{\&alpha;}}^{\frac{1}{2}}-1\left)\right)}^{-1};k_2={\left(8{\mathrm{\&alpha;}}^{\frac{3}{2}}\right({\mathrm{\&alpha;}}^{\frac{1}{2}}+1\left)\right)}^{-1}$

Step 2.2.2: measure and update

7. Cholesky decomposed P_xx:

$P_{xx}=S_{xx}{S}_{xx}^T$

8. volume point is calculated:

$y_{1,i}=\stackrel{\&OverBar;}{r_1}{S}_{xx}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k/k-1},y_{2,i}=\stackrel{\&OverBar;}{r_2}{S}_{xx}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k/k-1};$

$y_{3,j}=\stackrel{\&OverBar;}{r_1}{S}_{xx}{\&lsqb;s\&rsqb;}_j+{\hat{x}}_{k/k-1},y_{4,j}=\stackrel{\&OverBar;}{r_2}{S}_{xx}{\&lsqb;s\&rsqb;}_j+{\hat{x}}_{k/k-1}$

9. volume point is propagated by observational equation:

z_1,i=h (y_1,i), z_2,i=h (y_2,i)

z_3,i=h (y_3,i), z_4,i=h (y_4,i)

10. the observation predictive value in k moment is estimated:

${\hat{z}}_{k/k-1}=K\underset{i=1}{\overset{2n}{\&Sigma;}}(k_1{z}_{1,i}+k_2{z}_{2,i})+\underset{j=1}{\overset{2n(n-1)}{\&Sigma;}}(k_1{z}_{3,j}+k_2{z}_{4,j})$

11 calculate auto-correlation and cross-correlation covariance matrix:

$P_{zz}=-{\hat{z}}_{k/k-1}{\hat{z}}_{k/k-1}^T+K\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}\left(k_1{z}_{1,i}{z}_{1,i}^T+k_2{z}_{2,i}{z}_{2,i}^T\right)+\underset{j=1}{\overset{2n\left(n-1\right)}{\mathrm{\&Sigma;}}}\left(k_1{z}_{3,j}{z}_{3,j}^T+k_2{z}_{4,j}{z}_{4,j}^T\right)+R_k$

$P_{xz}=K\underset{i=1}{\overset{2n}{\&Sigma;}}(k_1{y}_{1,i}{z}_{1,i}^T+k_2{y}_{2,i}{z}_{2,i}^T)+\underset{j=1}{\overset{2n(n-1)}{\&Sigma;}}(k_1{y}_{3,j}{z}_{3,j}^T+k_2{y}_{4,j}{z}_{4,j}^T)$

12 utilize Guass-Bayes filter frame to complete filtering:

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})$

$P_k=P_{xx}-K_k{P}_{zz}{K}_k^T,K_k=P_{xz}{P}_{zz}^{-1}$

Owing to described measurement equation is linear equation, update so simplifying to measure:

5. Cholesky decomposed P_xx:

$P_{xx}=S_{xx}{S}_{xx}^T$

6. one-step prediction measuring value is calculated

${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$

7. auto-correlation and cross-correlation covariance matrix are calculated:

$P_{zz}=H_k{P}_{k/k-1}{H}_k^T+R_k$

$P_{xz}=P_{k/k-1}{H}_k^T$

8. Guass-Bayes filter frame is utilized to complete filtering:

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})$

$P_k=P_{xx}-K_k{P}_{zz}{K}_k^T,K_k=P_{xz}{P}_{zz}^{-1}.$

Further, described step 3) utilize the five rank CKF information that SINS and GPS, CNS are exported to be filtered, merge, obtain optimal estimation the erection rate information of navigational parameter, positional information and strapdown attitude matrix, to obtain accurate navigation information particularly as follows:

Step 3.1: after five rank CKF wave filter complete once filtering, the 12 dimension quantity of state estimated values according to wave filter output: $x=\left[\begin{array}{cccccccccccc}{\mathrm{\&delta;v}}_e^n& {\mathrm{\&delta;v}}_n^n& {\mathrm{\&phi;}}_e& {\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_u& \mathrm{\&delta;}L& \mathrm{\&delta;}\mathrm{\&lambda;}& {\&dtri;}_x^b& {\&dtri;}_y^b& {\mathrm{\&epsiv;}}_x^b& {\mathrm{\&epsiv;}}_y^b& {\mathrm{\&epsiv;}}_z^b\end{array}\right]$ Carry out velocity information, the correction of positional information and strapdown attitude matrix.

Step 3.2: according to obtained in step 3.1Carry out velocity information correction:

$V_{GPS}={\tilde{v}}_s^n+{\mathrm{\&delta;v}}_e^n$

Obtained speed is resolved, when GPS does not have signal the V calculated for SINS_GPSIt is used as real velocity information.

Step 3.3: according to φ obtained in step 3.1_e、φ_n、φ_uCarry out attitude information correction:

Calculating n ' is the attitude matrix between n systemComputational methods are as follows:

$C_n^{n^{\&prime;}}=\left[\begin{array}{ccc}{\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_u\_{\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_u+{\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& \_{\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_e\\ \_{\mathrm{cos\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_e{\mathrm{cos\&phi;}}_u& {\mathrm{sin\&phi;}}_e\\ {\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_u+{\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_u-{\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_e\end{array}\right]$

It it is the attitude matrix between b system according to n 'And n ' is and attitude matrix between n systemCalculate accurate strapdown attitude matrixComputational methods are as follows:

$C_b^n=C_b^{n^{\&prime;}}{\left(C_n^{n^{\&prime;}}\right)}^T$

According to strapdown attitude matrixCalculating Eulerian angles, computational methods are as follows:

$\left\{\begin{array}{c}\mathrm{\&theta;}=arcsin\left(C_b^n\right(2,3\left)\right)\\ \mathrm{\&gamma;}=\arctan (-C_b^n(1,3)/C_b^n(3,3\left)\right)\\ \mathrm{\&psi;}=arctan(-C_b^n(2,1)/C_b^n(2,2\left)\right)\end{array}\right.$

Step 3.4: carry out following location Information revision according to δ L obtained in step 3.1, δ λ:

$L^{\&prime;}=\tilde{L}+\mathrm{\&delta;}L$

${\mathrm{\&lambda;}}^{\&prime;}=\widetilde{\mathrm{\&lambda;}}+\mathrm{\&delta;}\mathrm{\&lambda;}$

Wherein,Being that SINS resolves obtained latitude, L ' is obtained accurate latitude after revising；Being that SINS resolves obtained latitude, λ ' is obtained accurate longitude after revising.

Beneficial effect: the present invention has the advantage that for prior art

1. the present invention according to SINS, GPS and CNS general principles and each pluses and minuses establish the error model of integrated navigation.Learning from other's strong points to offset one's weaknesses, the complementarity making full use of them completes integrated navigation task.

2. tradition CKF is very limited based on three rank volume criterion precision raisings, for solving this problem, proposes a kind of five rank CKF algorithms and be effectively increased CKF arithmetic accuracy and have fine filtration efficiency in literary composition on the basis of analysis high-order volume criterion.

3. the present invention uses GPS to provide velocity information, and CNS improves attitude angle information, is favorably improved filtering accuracy and the convergence rate of five rank CKF.

Accompanying drawing explanation

Fig. 1 is pitch angle Error Graph of the present invention.

Fig. 2 is roll angle Error Graph of the present invention.

Fig. 3 is course angle Error Graph of the present invention.

Fig. 4 is east orientation velocity error figure of the present invention.

Fig. 5 is north orientation velocity error figure of the present invention.

Fig. 6 is latitude error figure of the present invention.

Fig. 7 is longitude error figure of the present invention.

Fig. 8 is aircraft pathway figure of the present invention.

Fig. 9 is the system schema figure of the present invention.

Figure 10 is the present invention SINS/GPS/CNS Combinated navigation method schematic diagram based on five rank CKF

Figure 11 is the five rank CKF filter flow figure of the present invention

Detailed description of the invention

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

As shown in Figure 9 and Figure 10, it is the system schema figure and navigation principle figure of the present invention.

SINS strapdown settlement module collect Inertial Measurement Unit (IMU) module output gyro output valve and accelerometer output valve carry out strapdown resolving, obtain the information such as attitude angle, attitude matrix, speed, position；The velocity information of GPS device output carrier；CNS equipment output attitude information, SINS and GPS, the information of CNS output is input simultaneously in five rank CKF wave filter, and carries out the Filtering Processing of information, and filtering is as follows:

1., according to SINS/GPS/CNS integrated navigation characteristic, choose 12 dimension state vectors:

$x=\left[\begin{array}{cccccccccccc}{\mathrm{\&delta;v}}_e^n& {\mathrm{\&delta;v}}_n^n& {\mathrm{\&phi;}}_e& {\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_u& \mathrm{\&delta;}L& \mathrm{\&delta;}\mathrm{\&lambda;}& {\&dtri;}_x^b& {\&dtri;}_y^b& {\mathrm{\&epsiv;}}_x^b& {\mathrm{\&epsiv;}}_y^b& {\mathrm{\&epsiv;}}_z^b\end{array}\right]$

Set up state equation:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=C_{\mathrm{\&omega;}}^{-1}\&lsqb;\left(I-C_n^{n^{\&prime;}}\right){\widetilde{\mathrm{\&omega;}}}_{in}^n+{\mathrm{\&delta;\&omega;}}_{in}^n-C_b^{n^{\&prime;}}{\mathrm{\&epsiv;}}^b\&rsqb;+w_g^n\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}^n=\left(I-C_{n^{\&prime;}}^n\right)C_b^{n^{\&prime;}}{\tilde{f}}^b-\left(2{\widetilde{\mathrm{\&omega;}}}_{ie}^n+{\widetilde{\mathrm{\&omega;}}}_{en}^n\right)\&times;\mathrm{\&delta;}{\tilde{v}}^n+C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}{\&dtri;}^b+w_a^n\\ \mathrm{\&delta;}L=\frac{{\hat{v}}_N^n}{R_M}-\frac{{\hat{v}}_N^n-{\mathrm{\&delta;v}}_N^n}{R_M-{\mathrm{\&delta;R}}_M}\\ \mathrm{\&delta;}\mathrm{\&lambda;}=\frac{{\hat{v}}_E^n\sec \hat{L}}{R_N}-\frac{\left({\hat{v}}_N^n-\mathrm{\&delta;}{\hat{v}}_N^n\right)\sec \left(\hat{L}-\mathrm{\&delta;}L\right)}{R_N-{\mathrm{\&delta;R}}_N}\\ {\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}^b=0\\ {\stackrel{\&CenterDot;}{\&dtri;}}^b=0\end{array}\right.,$

Discretization postscript is: x_k=f (x_k-1)+ω_k-1.

2. access speed amount and attitude angle are measuring value, set up and measure filtering equations:

$Z\left(t\right)=\left[\begin{array}{c}{H}_{\mathrm{\&phi;}}\left(t\right)\\ H_v\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_{\mathrm{\&phi;}}\left(t\right)\\ V_v\left(t\right)\end{array}\right]=H\left(t\right)X\left(t\right)+V\left(t\right)$

Discretization postscript is: x_k=f (x_k-1)+ω_k-1

3. the time updates

1. the Posterior probability distribution in k-1 moment is assumedIt is known that Cholesky decomposes:

P_k-1=S_k-1S_k-1 ^T

2. volume point is calculated:

$x_{1,i}=\stackrel{\&OverBar;}{r_1}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1},x_{2,i}=\stackrel{\&OverBar;}{r_2}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1}$

$x_{3,i}=\stackrel{\&OverBar;}{r_1}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1},x_{4,i}=\stackrel{\&OverBar;}{r_2}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1}$

In formula, i=1,2 ..., 2n；J=1,2 ..., 2n (n-1)

3. volume point is propagated by state equation:

x_1,i ^*=f (x_1,i)；x_2,i ^*=f (x_2,i)

x_3,j ^*=f (x_3,j)；x_4,j ^*=f (x_4,j)

4. the predictive value of k moment state and error battle array is estimated:

${\hat{x}}_{k/k-1}=K\underset{i=1}{\overset{2n}{\&Sigma;}}(k_1{x^{*}}_{1,i}+k_2{x^{*}}_{2,i})+\underset{j=1}{\overset{2n(n-1)}{\&Sigma;}}(k_1{x^{*}}_{3,j}+k_2{x^{*}}_{4,j})$

$P_{xx}=K\underset{i=1}{\overset{2n}{\&Sigma;}}(k_1{x}_{1,i}^{*}{x_{1,i}^{*}}^T+k_2{x}_{2,i}^{*}{x_{2,i}^{*}}^T)-{\hat{x}}_{k/k-1}{\hat{x}}_{k/k-1}^T+\underset{i=1}{\overset{2n(n-1)}{\&Sigma;}}(k_1{x}_{3,j}^{*}{x_{3,j}^{*}}^T+k_2{x}_{4,j}^{*}{x_{4,j}^{*}}^T)+Q_{k-1}$

$K=3-\mathrm{\&alpha;};k_1={\left(8{\mathrm{\&alpha;}}^{\frac{3}{2}}\right({\mathrm{\&alpha;}}^{\frac{1}{2}}-1\left)\right)}^{-1};k_2={\left(8{\mathrm{\&alpha;}}^{\frac{3}{2}}\right({\mathrm{\&alpha;}}^{\frac{1}{2}}+1\left)\right)}^{-1}$

4. measure and update

1. Cholesky decomposed P_xx:

$P_{xx}=S_{xx}{S}_{xx}^T$

2. one-step prediction measuring value is calculated

${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$

3. auto-correlation and cross-correlation covariance matrix are calculated:

$P_{zz}=H_k{P}_{k/k-1}{H}_k^T+R_k$

$P_{xz}=P_{k/k-1}{H}_k^T$

4. Guass-Bayes filter frame is utilized to complete filtering:

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})$

$P_k=P_{xx}-K_k{P}_{zz}{K}_k^T;K_k=P_{xz}{P}_{zz}^{-1}$

5. after five rank CKF wave filter complete once filtering, the 12 dimension quantity of state estimated values according to wave filter output: $x=\left[\begin{array}{cccccccccccc}{\mathrm{\&delta;v}}_e^n& {\mathrm{\&delta;v}}_n^n& {\mathrm{\&phi;}}_e& {\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_u& \mathrm{\&delta;}L& \mathrm{\&delta;}\mathrm{\&lambda;}& {\&dtri;}_x^b& {\&dtri;}_y^b& {\mathrm{\&epsiv;}}_x^b& {\mathrm{\&epsiv;}}_y^b& {\mathrm{\&epsiv;}}_z^b\end{array}\right]$ Carry out velocity information, the correction of positional information and strapdown attitude matrix.

1. according to obtainedCarry out velocity information correction:

$V_{GPS}={\tilde{v}}_s^n+{\mathrm{\&delta;v}}_e^n$

Obtained speed is resolved, when GPS does not have signal the V calculated for SINS_GPSIt is used as real velocity information.

2. according to obtained φ_e、φ_n、φ_uCarry out attitude information correction:

Calculating n ' is the attitude matrix between n systemComputational methods are as follows:

$C_n^{n^{\&prime;}}=\left[\begin{array}{ccc}{\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_u\_{\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_u+{\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& \_{\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_e\\ \_{\mathrm{cos\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_e{\mathrm{cos\&phi;}}_u& {\mathrm{sin\&phi;}}_e\\ {\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_u+{\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_u-{\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_e\end{array}\right]$

It it is the attitude matrix between b system according to n 'And n ' is and attitude matrix between n systemCalculate accurate strapdown attitude matrixComputational methods are as follows:

$C_b^n=C_b^{n^{\&prime;}}{\left(C_n^{n^{\&prime;}}\right)}^T$

According to strapdown attitude matrixCalculating Eulerian angles, computational methods are as follows:

$\left\{\begin{array}{c}\mathrm{\&theta;}=arcsin\left(C_b^n\right(2,3\left)\right)\\ \mathrm{\&gamma;}=\arctan (-C_b^n(1,3)/C_b^n(3,3\left)\right)\\ \mathrm{\&psi;}=arctan(-C_b^n(2,1)/C_b^n(2,2\left)\right)\end{array}\right.$

3. positional information correction is carried out according to obtained δ L, δ λ:

$L^{\&prime;}=\tilde{L}+\mathrm{\&delta;}L$

${\mathrm{\&lambda;}}^{\&prime;}=\widetilde{\mathrm{\&lambda;}}+\mathrm{\&delta;}\mathrm{\&lambda;}$

Wherein,Being that SINS resolves obtained latitude, L ' is obtained accurate latitude after revising；Being that SINS resolves obtained latitude, λ ' is obtained accurate longitude after revising.

Use following example that beneficial effects of the present invention is described:

1) kinematic parameter is arranged:

Emulation initial time aircraft is at north latitude 32 degree, 118 degree of places of east longitude；And do twin shaft oscillating motion rotating around pitch axis, axis of roll with SIN function, pitch angle θ, roll angle γ amplitude of waving be respectively as follows: 9,12, rolling period is respectively as follows: 16s, 20s, angle, initial heading is 30 degree, and initial misalignment is: Δ θ=15 °, Δ γ=15 °, Δ ψ=100 °；Emulation this aircraft of initial time, with the speed of 10m/s, moves with uniform velocity, and as shown in Figure 8, speed when turning round is 9 °/s to motion path, hours underway 6000s.

2) parameter of sensor is arranged:

The gyroscope constant value drift of the strapdown system of aircraft is 0.01 °/h:, Modelling of Random Drift of Gyroscopes is 0.01 °/h:, accelerometer is biased to: 50mg, and accelerometer random error is: 50mg, GPS output speed error is: 0.1m/s.

It is 1 point that CNS exports attitude error.

3) analysis of simulation result:

Using the Combinated navigation method based on the SINS/GPS/CNS of five rank CKF provided by the invention to emulate, Fig. 1,2,3 is pitch angle error in navigation procedure of the present invention, roll angle error, and the curve of course angle error；Fig. 4,5 is east orientation and north orientation speed-error curve in navigation procedure of the present invention；Fig. 6,7 is navigation procedure middle latitude of the present invention and longitude error curve.It appeared that: after aircraft flight 1000s, pitch angle is restrained substantially, error about 0.07 °；After 500s, roll angle restrains substantially, error about 0.07 °；After 100s, course angle restrains substantially, error about 0.02 °.After 1550s, east orientation speed restrains substantially, and error is 0.12m/s.After 1500s, north orientation speed restrains substantially, and error is 0.1m/s.Latitude, longitude error is always only small, and maximum latitude error is 26 points, and maximum longitude error is 0.031 point.By analyze find, navigation 6000s after, pitch angle error 0.0002 ° within, roll angle error is within 0.0009 °, and course angle error is within 0.02；East orientation velocity error is within 0.01m/s, and north orientation velocity error is within 0.033m/s, and latitude error is within 1.63 points, and longitude error is within 0.0064 point.

Result above shows, five rank CKF algorithms are for making systematic error Fast Convergent and stable within less range of error in SINS/GPS/CNS integrated navigation.

The preferred embodiment of the present invention described in detail above; but, the present invention is not limited to the detail in above-mentioned embodiment, in the technology concept of the present invention; technical scheme can being carried out multiple equivalents, these equivalents belong to protection scope of the present invention.

Claims (4)

1. the GPS/SINS/CNS Combinated navigation method based on five rank CKF, it is characterised in that comprise the steps:

Step 1: set up the integrated navigation system nonlinearity erron equation based on SINS error equation and linear measurement equation；

Step 2: utilize five rank spheres radially volume rule criteria construction five rank volume Kalman filter；

Step 3: utilize the five rank CKF information that SINS and GPS, CNS are exported to be filtered, merge, obtain optimal estimation the erection rate information of navigational parameter, positional information and strapdown attitude matrix, to obtain accurate navigation information.

2. the GPS/SINS/CNS Combinated navigation method based on five rank CKF according to claim 1, it is characterised in that described step 1 set up the integrated navigation system nonlinearity erron equation based on SINS error equation and linear measurement equation particularly as follows:

Step 1.1: choosing " sky, northeast " geographic coordinate system is navigational coordinate system n system；Choosing carrier " on before the right side " coordinate system is carrier coordinate system b system；N system successively turns to b system through 3 Eulerian angles, three Eulerian angles be designated as course angle ψ ∈ (-π, π], pitch angleRoll angle γ ∈ (-π, π]；Attitude matrix between n system and b system is designated asTrue attitude angle is designated asTrue velocity is designated as $v_s^n={\left[\begin{array}{ccc}{v}_e^n& v_n^n& v_u^n\end{array}\right]}^T;$ True geographic coordinate system is P=[L λ H]^T, wherein, L is latitude, and λ is longitude, and H is height；The attitude angle that SINS calculates is designated asSpeed is designated as ${\tilde{v}}_s^n={\left[\begin{array}{ccc}{\tilde{v}}_e^n& {\tilde{v}}_n^n& {\tilde{v}}_u^n\end{array}\right]}^T,$ Geographic coordinate system is $\tilde{P}={\left[\begin{array}{ccc}\tilde{L}& \widetilde{\mathrm{\&lambda;}}& \tilde{H}\end{array}\right]}^T;$ The mathematical platform that SINS calculates is designated as n ', and the attitude matrix that n ' is between b system is designated asNote attitude error isVelocity error is: ${\mathrm{\&delta;v}}^n=v_s^n-{\tilde{v}}_s^n={\left[\begin{array}{ccc}{\mathrm{\&delta;v}}_e^n& {\mathrm{\&delta;v}}_n^n& {\mathrm{\&delta;v}}_u^n\end{array}\right]}^T,$ Site error is: $\mathrm{\&delta;}P=P-\tilde{P}=\&lsqb;\begin{array}{ccc}\mathrm{\&delta;}L& \mathrm{\&delta;}\mathrm{\&lambda;}& \mathrm{\&delta;}H\end{array}\&rsqb;.$ Then Nonlinear Error Models is as follows:

$\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=C_{\mathrm{\&omega;}}^{-1}\&lsqb;(I-C_n^{n^{\&prime;}}){\widetilde{\mathrm{\&omega;}}}_{in}^n+C_n^{n^{\&prime;}}{\mathrm{\&delta;\&omega;}}_{in}^b-C_b^{n^{\&prime;}}({\mathrm{\&epsiv;}}^b+w_g^b)\&rsqb;$

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}^n=\&lsqb;(I-C_{n^{\&prime;}}^n)\&rsqb;C_b^{n^{\&prime;}}{\tilde{f}}^b-(2{\mathrm{\&delta;\&omega;}}_{ie}^n+{\mathrm{\&delta;\&omega;}}_{en}^n)\&times;{\tilde{v}}^n-(2{\widetilde{\mathrm{\&omega;}}}_{ie}^n+{\widetilde{\mathrm{\&omega;}}}_{en}^n)\&times;{\mathrm{\&delta;v}}^n+C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}({\&dtri;}^b+w_a^b)$

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\frac{v_n^n}{R_M+H}-\frac{{\tilde{v}}_n^n}{R_M+H}=\frac{{\mathrm{\&delta;v}}_n^n}{R_M+H}$

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{v_E^n\sec L}{R_N+H}-\frac{{\tilde{v}}_E^n\sec \tilde{L}}{R_N+H}$

Wherein, ${\mathrm{\&epsiv;}}^b=\&lsqb;\begin{array}{ccc}{\mathrm{\&epsiv;}}_x^b& {\mathrm{\&epsiv;}}_y^b& {\mathrm{\&epsiv;}}_z^b\end{array}\&rsqb;$ The constant error of three axle gyros is descended for b system,Random error for the lower three axle gyros of b system is zero-mean white-noise process, ${\&dtri;}^b={\left[\begin{array}{ccc}{\&dtri;}_x^b& {\&dtri;}_y^b& {\&dtri;}_z^b\end{array}\right]}^T$ 3-axis acceleration constant error is descended for b system,Random error for the lower 3-axis acceleration of n system is zero-mean white-noise process,For the actual output of accelerometer,It is mathematical platform angular velocity of rotation under b system,For the mathematical platform angular velocity of rotation calculated,For earth rotation angular velocity,For angular velocity of rotation relative to the earth of the mathematical platform that calculates,For correspondenceCalculating error.R_M、R_NRespectively local west, meridian circle fourth of the twelve Earthly Branches circle radius of curvature,Inverse matrix for Eulerian angles differential equation coefficient matrix；It is the attitude matrix between n system for n ', C_ωWithParticularly as follows:

$C_n^{n^{\&prime;}}=\left[\begin{array}{ccc}{\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_u\_{\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_u+{\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& \_{\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_e\\ \_{\mathrm{cos\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_e{\mathrm{cos\&phi;}}_u& {\mathrm{sin\&phi;}}_e\\ {\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_u+{\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_u\_{\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_e\end{array}\right]$

$C_{\mathrm{\&omega;}}=\left[\begin{array}{ccc}{\mathrm{cos\&phi;}}_n& 0& -{\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_e\\ 0& 1& {\mathrm{sin\&phi;}}_e\\ {\mathrm{sin\&phi;}}_n& 0& {\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_e\end{array}\right];$

Step 1.2: it is as follows that described nonlinearity erron equation and linear measurement equation set up process:

Access speed errorEulerian angles platform error angle φ_e、φ_n、φ_u；Site error δ L, δ λ；Accelerometer constant error under b systemAnd gyroscope constant value errorAnd constitute 12 dimension state vectors,H is approximately zero:

$x=\&lsqb;\begin{array}{cccccccccccc}{\mathrm{\&delta;v}}_e^n& {\mathrm{\&delta;v}}_n^n& {\mathrm{\&phi;}}_e& {\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_u& \mathrm{\&delta;}L& \mathrm{\&delta;}\mathrm{\&lambda;}& {\&dtri;}_x^b& {\&dtri;}_y^b& {\mathrm{\&epsiv;}}_x^b& {\mathrm{\&epsiv;}}_y^b& {\mathrm{\&epsiv;}}_z^b\end{array}\&rsqb;$

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=C_{\mathrm{\&omega;}}^{-1}\&lsqb;(I-C_n^{n^{\&prime;}}){\widetilde{\mathrm{\&omega;}}}_{in}^n+{\mathrm{\&delta;\&omega;}}_{in}^n-C_b^{n^{\&prime;}}{\mathrm{\&epsiv;}}^b\&rsqb;+w_g^n\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}^n=(I-C_{n^{\&prime;}}^n)C_b^{n^{\&prime;}}{\tilde{f}}^b-(2{\widetilde{\mathrm{\&omega;}}}_{ie}^n+{\widetilde{\mathrm{\&omega;}}}_{en}^n)\&times;\mathrm{\&delta;}{\tilde{v}}^n+C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}{\&dtri;}^b+w_a^n\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\frac{{\mathrm{\&delta;v}}_n^n}{R_M}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{v_E^n\sec L}{R_N}-\frac{{\tilde{v}}_E^n\sec \tilde{L}}{R_N}\\ {\stackrel{\&CenterDot;}{\&dtri;}}^b=0\\ {\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}^b=0\end{array}\right.$

Front bidimensional state is only taken with P,WithFor zero-mean white-noise process；.This nonlinear state equation can be abbreviated as:Using sampling period T as filtering the cycle, it is possible to use quadravalence Runge-Kutta integration method, with T for step-length by its discretization, specifically comprise the following steps that

Assume the state vector the chosen value x at initial time₀It is known that and note t₁=t+T/2, then can be incited somebody to action by following iterative formulaDiscrete:

Δx₁=[f (x, t)+ω (t)] T

Δx₂=[f (x, t₁)+Δx₁/2+ω(t₁)]T

Δx₃=[f (x, t₁)+Δx₂/2+ω(t₁)]T

Δx₄=[f (x, t+T)+Δ x₃+ω(t+T)]T

x_k+1=x_k+(Δx₁+2Δx₂+2Δx₃+Δx₄)/6

Remember that discrete rear state filtering equation is: x_k=f (x_k-1)+ω_k-1

It is as follows that linear measurement equation sets up process:

Choose GPS velocity errorM_e、M_nThe respectively range rate error item of GPS component on east orientation north orientation coordinate axes；

Velocity measurement amount vector definition is as follows:

$Z_v\left(t\right)=\left[\begin{array}{c}{\mathrm{\&delta;v}}_e+M_e\\ {\mathrm{\&delta;v}}_n+M_n\end{array}\right]=H_v\left(t\right)X\left(t\right)+V_v\left(t\right)$

In formula, H_v(t)=[diag [1,1] 0_2×10]

Under strapdown mode of operation, CNS the attitude information exported can obtain the three-axis attitude information of carrierAnd the three-axis attitude information that SINS also can provide carrier by inertial reference calculation isTherefore both are subtracted each other the three-axis attitude error angle Δ φ that can obtain carrier:

$\mathrm{\&Delta;}\mathrm{\&phi;}=\left[\begin{array}{c}{\mathrm{\&Delta;\&phi;}}_e\\ {\mathrm{\&Delta;\&phi;}}_n\\ {\mathrm{\&Delta;\&phi;}}_u\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&theta;}}_0-\widetilde{\mathrm{\&theta;}}\\ {\mathrm{\&gamma;}}_0-\widetilde{\mathrm{\&gamma;}}\\ {\mathrm{\&psi;}}_0-\widetilde{\mathrm{\&psi;}}\end{array}\right]$

Δ φ '=M × Δ φ

In formula, M is attitude error angle transition matrix:

$M=\left[\begin{array}{ccc}0& {\mathrm{cos\&theta;}}_0& -{\mathrm{cos\&gamma;}}_0{\mathrm{sin\&theta;}}_0\\ 0& {\mathrm{sin\&theta;}}_0& {\mathrm{cos\&gamma;}}_0{\mathrm{sin\&theta;}}_0\\ 1& 0& {\mathrm{sin\&gamma;}}_0\end{array}\right]$

Δ φ ' is set up as observation the measurement model of system by integrated navigation system, estimates the error of inertia device in navigation system in real time by the method for optimal estimation, and with this, SINS is modified；

Measurement equation is as follows:

$Z_{\mathrm{\&phi;}}\left(t\right)=\left[\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&theta;}}^{\&prime;}\\ \mathrm{\&Delta;}{\mathrm{\&gamma;}}^{\&prime;}\\ \mathrm{\&Delta;}{\mathrm{\&psi;}}^{\&prime;}\end{array}\right]=H_{\mathrm{\&phi;}}X\left(t\right)+V_{\mathrm{\&phi;}}\left(t\right)$

In formula, H_φ=[0_3×2I_3×30_3×7]

By analyzing above it can be seen that the measurement equation of full combination is:

$Z\left(t\right)=\left[\begin{array}{c}{H}_{\mathrm{\&phi;}}\left(t\right)\\ H_v\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_{\mathrm{\&phi;}}\left(t\right)\\ V_v\left(t\right)\end{array}\right]=H\left(t\right)X\left(t\right)+V\left(t\right)$

Same using T as the filtering cycle, and carry out simple discretization using T as step-length.

By state equation and measurement equation filtering equations composed as follows:

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+{\mathrm{\&omega;}}_{k-1}\\ z_k=h\left(k\right)x_k+v_k\end{array}\right..$

3. the GPS/SINS/CNS Combinated navigation method based on five rank CKF according to claim 1, it is characterised in that described step 2) utilize five rank spheres radially volume rule criteria construction five rank volume Kalman filter particularly as follows:

Step 2.1: radially volume criterion is as follows for five rank spheres:

$I\left(f\right)={\&Integral;}_{R^n}f\left(x\right)\exp (-x^T{x}^T)dx$

F (x) is arbitrary function, RⁿFor integral domain, the method that use is approximate obtains the approximate solution of its analytic value.

Take x=ry (y^TY=1, r ∈ [0, ∞)), therefore above-mentioned integration can be write as in sphere radial coordinate system:

$I\left(f\right)=\underset{0}{\overset{\mathrm{\&infin;}}{\&Integral;}}\underset{U_n}{\&Integral;}f\left(ry\right)r^{n-1}{e}^{-r^2}d\mathrm{\&sigma;}\left(y\right)dr$

U_nFor n N-dimension unit sphere, σ () is U_nOn element.Integration I (f) is separated into Spherical integration and Radial amasss:

$S\left(r\right)={\&Integral;}_{U_n}f\left(ry\right)d\mathrm{\&sigma;}\left(y\right);R={\&Integral;}_0^{\mathrm{\&infin;}}S\left(r\right)r^{n-1}{e}^{-r^2}dr$

Assume the N of Spherica integration S (r)_sSampling approaches:

$S\left(r\right)\&ap;\underset{j=1}{\overset{N_s}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_{r,j}f\left({\mathrm{ry}}_j\right)$

The N of Radial integration R_rSampling approaches:

$R=\underset{i=1}{\overset{N_r}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_{r,i}S\left(r_i\right)$

Obtain the N of I (f)_r×N_sSampling approach into:

$I\left(f\right)\&ap;\underset{i=1}{\overset{N_r}{\&Sigma;}}{\mathrm{\&omega;}}_{r,i}{\mathrm{\&omega;}}_{s,j}f\left(r_i{y}_j\right)$

The computing formula of Spherical criterion: S (r) is as follows:

$S\left(r\right)={\mathrm{\&omega;}}_1\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}f\left(r{\&lsqb;e\&rsqb;}_i\right)+{\mathrm{\&omega;}}_2\underset{i=1}{\overset{2n(n-1)}{\mathrm{\&Sigma;}}}f\left(r{\&lsqb;s\&rsqb;}_i\right)$

Wherein ${\left[e\right]}_n=\{\&PlusMinus;{\left\{e\right\}}_{i-1}^n\},$ [s]_nFor [e]_nPoint Set, be designated as ${\left[s\right]}_n=\{\&PlusMinus;{\left\{s_i\right\}}_{i=1}^{n(n-1)}\},$ It is respectively as follows:

${\left\{e_i\right\}}_{i-1}^n={\{{\&lsqb;1,0,...,0\&rsqb;}^T,{\&lsqb;0,1,...,0\&rsqb;}^T,...,{\&lsqb;0,0,...,1\&rsqb;}^T\}}^T$

${\left\{s_i\right\}}_{i=1}^{n(n-1)}=\{(e_k+e_l)/\sqrt{2}:k,l=1,2,...,n;k<l\},$

[e]_n[s]_nInterior vector number respectively 2n and 2n (n-1), [e]_i[s]_iRespectively [e]_n[s]_nI-th row；

$f=1\&DoubleRightArrow;A_n=2{\mathrm{n\&omega;}}_1+2n(n-1){\mathrm{\&omega;}}_2$

$f=y_1^2\&DoubleRightArrow;A_n/n=2{\mathrm{\&omega;}}_1+2(n-1){\mathrm{\&omega;}}_2$

$f=y_1^2{y}_2^2\&DoubleRightArrow;A_n/\&lsqb;n(n+2)\&rsqb;={\mathrm{\&omega;}}_2$

$f=y_1^4\&DoubleRightArrow;3A_n/\&lsqb;n(n+2)\&rsqb;=2{\mathrm{\&omega;}}_1+(n-1){\mathrm{\&omega;}}_2$

Solve: ${\mathrm{\&omega;}}_1=\frac{(4-n)A_n}{2n(n+2)};{\mathrm{\&omega;}}_2=\frac{A_n}{n(n+2)}$

Radial rule:

$S\left(r\right)=1\&DoubleRightArrow;{\mathrm{\&omega;}}_{r,1}{r}_1^0+{\mathrm{\&omega;}}_{r,2}{r}_2^0=\mathrm{\&Gamma;}(n/2)/2$

$S\left(r\right)=r^2\&DoubleRightArrow;{\mathrm{\&omega;}}_{r,1}{r}_1^2+{\mathrm{\&omega;}}_{r,2}{r}_2^2=\mathrm{\&Gamma;}(n/2+1)/2$

$S\left(r\right)=r^4\&DoubleRightArrow;{\mathrm{\&omega;}}_{r,1}{r}_1^4+{\mathrm{\&omega;}}_{r,2}{r}_2^4=\mathrm{\&Gamma;}(n/2+2)/2$

Utilize general Gauss-Laguerre Integral Rule to solve equation group

${\mathrm{\&omega;}}_{r,1}=\mathrm{\&Gamma;}\left(\frac{n}{2}\right)\frac{\sqrt{\mathrm{\&alpha;}}+1}{4\sqrt{\mathrm{\&alpha;}}};{\mathrm{\&omega;}}_{r,2}=\mathrm{\&Gamma;}\left(\frac{n}{2}\right)\frac{\sqrt{\mathrm{\&alpha;}}-1}{4\sqrt{\mathrm{\&alpha;}}};\mathrm{\&alpha;}=\frac{n}{2}+1$

Step 2.2: the construction method of five rank volume Kalman filter is as follows:

Filtering equations for setting up:

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+{\mathrm{\&omega;}}_{k-1}\\ z_k=h\left(k\right)x_k+v_k\end{array}\right.$

X in formula_kIt is 12 dimension state vectors, and each component is independent, Gaussian distributed；z_kIt is 5 dimension observation vectors；ω_k-1And ν_kRespectively process noise and measurement noise, meets:

$\left\{\begin{array}{cc}E\&lsqb;{\mathrm{\&omega;}}_k\&rsqb;=0,& E\&lsqb;{\mathrm{\&omega;}}_i{{\mathrm{\&omega;}}_j}^T\&rsqb;=Q_k{\mathrm{\&delta;}}_{ij}\\ E\&lsqb;v_k\&rsqb;=0,& E\&lsqb;v_i{v_j}^T\&rsqb;=R_k{\mathrm{\&delta;}}_{ij}\\ E\&lsqb;{\mathrm{\&omega;}}_i{v_j}^T\&rsqb;=0& \end{array}\right.$

In formula, δ_ijFor delta-function；Q is the variance matrix of system noise；R is the variance matrix of measurement noise.

Five rank CKF algorithms are as follows:

Step 2.2.1: the time updates:

1. the Posterior probability distribution in k-1 moment is assumedIt is known that Cholesky decomposes:

P_k-1=S_k-1S_k-1 ^T

2. volume point is calculated:

$x_{1,i}=\stackrel{-}{r_1}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1},x_{2,i}=\stackrel{-}{r_2}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1}$

$x_{3,i}=\stackrel{-}{r_1}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1},x_{4,i}=\stackrel{-}{r_2}{S}_{k-1}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k-1}$

In formula, i=1,2 ..., 2n；J=1,2 ..., 2n (n-1)

3. volume point is propagated by state equation:

x_1,i ^*=f (x_1,i)；x_2,i ^*=f (x_2,i)

x_3,j ^*=f (x_3,j)；x_4,j ^*=f (x_4,j)

4. the predictive value of k moment state and error battle array is estimated:

${\hat{x}}_{k/k-1}=K\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}(k_1{x^{*}}_{1,i}+k_2{x^{*}}_{2,i})+\underset{j=1}{\overset{2n(n-1)}{\mathrm{\&Sigma;}}}(k_1{x^{*}}_{3,j}+k_2{x^{*}}_{4,j})$

$P_{xx}=K\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}(k_1{x}_{1,i}^{*}{x_{1,i}^{*}}^T+k_2{x}_{2,i}^{*}{x_{2,i}^{*}}^T)-{\hat{x}}_{k/k-1}{\hat{x}}_{k/k-1}^T+\underset{i=1}{\overset{2n(n-1)}{\mathrm{\&Sigma;}}}(k_1{x}_{3,j}^{*}{x_{3,j}^{*}}^T+k_2{x}_{4,j}^{*}{x_{4,j}^{*}}^T)+Q_{k-1}$

$K=3-\mathrm{\&alpha;};k_1={\left(8{\mathrm{\&alpha;}}^{\frac{3}{2}}\right({\mathrm{\&alpha;}}^{\frac{1}{2}}-1\left)\right)}^{-1};k_2={\left(8{\mathrm{\&alpha;}}^{\frac{3}{2}}\right({\mathrm{\&alpha;}}^{\frac{1}{2}}+1\left)\right)}^{-1}$

Step 2.2.2: measure and update

1. Cholesky decomposed P_xx:

$P_{xx}=S_{xx}{S}_{xx}^T$

2. volume point is calculated:

$y_{1,i}=\stackrel{\&OverBar;}{r_1}{S}_{xx}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k/k-1},y_{2,i}=\stackrel{\&OverBar;}{r_2}{S}_{xx}{\&lsqb;e\&rsqb;}_i+{\hat{x}}_{k/k-1};$

$y_{3,j}=\stackrel{\&OverBar;}{r_1}{S}_{xx}{\&lsqb;s\&rsqb;}_j+{\hat{x}}_{k/k-1},y_{4,j}=\stackrel{\&OverBar;}{r_2}{S}_{xx}{\&lsqb;s\&rsqb;}_j+{\hat{x}}_{k/k-1}$

3. volume point is propagated by observational equation:

z_1,i=h (y_1,i), z_2,i=h (y_2,i)

z_3,i=h (y_3,i), z_4,i=h (y_4,i)

4. the observation predictive value in k moment is estimated:

${\hat{z}}_{k/k-1}=K\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}(k_1{z}_{1,i}+k_2{z}_{2,i})+\underset{j=1}{\overset{2n(n-1)}{\mathrm{\&Sigma;}}}(k_1{z}_{3,j}+k_2{z}_{4,j})$

5. auto-correlation and cross-correlation covariance matrix are calculated:

$P_{zz}=-{\hat{z}}_{k/k-1}{\hat{z}}_{k/k-1}^T+K\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}(k_1{z}_{1,i}{z}_{1,i}^T+k_2{z}_{2,i}{z}_{2,i}^T)+\underset{j=1}{\overset{2n(n-1)}{\mathrm{\&Sigma;}}}(k_1{z}_{3,j}{z}_{3,j}^T+k_2{z}_{4,j}{z}_{4,j}^T)+R_k$

$P_{xz}=K\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}(k_1{y}_{1,i}{z}_{1,i}^T+k_2{y}_{2,i}{z}_{2,i}^T)+\underset{j=1}{\overset{2n(n-1)}{\mathrm{\&Sigma;}}}(k_1{y}_{3,j}{z}_{3,j}^T+k_2{y}_{4,j}{z}_{4,j}^T)$

6. Guass-Bayes filter frame is utilized to complete filtering:

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})$

$P_k=P_{xx}-K_k{P}_{zz}{K}_k^T,K_k=P_{xz}{P}_{zz}^{-1}$

Owing to described measurement equation is linear equation, update so simplifying to measure:

1. Cholesky decomposed P_xx:

$P_{xx}=S_{xx}{S}_{xx}^T$

2. one-step prediction measuring value is calculated

${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$

3. auto-correlation and cross-correlation covariance matrix are calculated:

$P_{zz}=H_k{P}_{k/k-1}{H}_k^T+R_k$

$P_{xz}=P_{k/k-1}{H}_k^T$

4. Guass-Bayes filter frame is utilized to complete filtering:

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})$

$P_k=P_{xx}-K_k{P}_{zz}{K}_k^T,K_k=P_{xz}{P}_{zz}^{-1}.$

4. a kind of GPS/SINS/CNS Combinated navigation method based on five rank CKF as claimed in claim 1, it is characterized in that described step 3) utilize the five rank CKF information that SINS and GPS, CNS are exported to be filtered, merge, obtain optimal estimation the erection rate information of navigational parameter, positional information and strapdown attitude matrix, to obtain accurate navigation information particularly as follows:

Step 3.1: after five rank CKF wave filter complete once filtering, the 12 dimension quantity of state estimated values according to wave filter output: $x=\&lsqb;\begin{array}{cccccccccccc}{\mathrm{\&delta;v}}_e^n& {\mathrm{\&delta;v}}_n^n& {\mathrm{\&phi;}}_e& {\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_u& \mathrm{\&delta;}L& \mathrm{\&delta;}\mathrm{\&lambda;}& {\&dtri;}_x^b& {\&dtri;}_y^b& {\mathrm{\&epsiv;}}_x^b& {\mathrm{\&epsiv;}}_y^b& {\mathrm{\&epsiv;}}_z^b\end{array}\&rsqb;$ Carry out velocity information, the correction of positional information and strapdown attitude matrix.

Step 3.2: according to obtained in step 3.1Carry out velocity information correction:

$V_{GPS}={\tilde{v}}_s^n+{\mathrm{\&delta;v}}_e^n$

Obtained speed is resolved, when GPS does not have signal the V calculated for SINS_GPSIt is used as real velocity information.

Step 3.3: according to φ obtained in step 3.1_e、φ_n、φ_uCarry out attitude information correction:

Calculating n ' is the attitude matrix between n systemComputational methods are as follows:

$C_n^{n^{\&prime;}}=\left[\begin{array}{ccc}{\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_u-{\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_u+{\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& -{\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_e\\ -{\mathrm{cos\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_e{\mathrm{cos\&phi;}}_u& {\mathrm{sin\&phi;}}_e\\ {\mathrm{sin\&phi;}}_n{\mathrm{cos\&phi;}}_u+{\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{sin\&phi;}}_n{\mathrm{sin\&phi;}}_u-{\mathrm{cos\&phi;}}_n{\mathrm{sin\&phi;}}_e{\mathrm{sin\&phi;}}_u& {\mathrm{cos\&phi;}}_n{\mathrm{cos\&phi;}}_e\end{array}\right]$

It it is the attitude matrix between b system according to n 'And n ' is and attitude matrix between n systemCalculate accurate strapdown attitude matrixComputational methods are as follows:

$C_b^n=C_b^{n^{\&prime;}}{\left(C_n^{n^{\&prime;}}\right)}^T$

According to strapdown attitude matrixCalculating Eulerian angles, computational methods are as follows:

$\left\{\begin{array}{c}\mathrm{\&theta;}=\arcsin \left(C_b^n\right(2,3\left)\right)\\ \mathrm{\&gamma;}=\arctan (-C_b^n(1,3)/C_b^n(3,3\left)\right)\\ \mathrm{\&psi;}=\arctan (-C_b^n(2,1)/C_b^n(2,2\left)\right)\end{array}\right.$

Step 3.4: carry out following location Information revision according to δ L obtained in step 3.1, δ λ:

$L^{\&prime;}=\tilde{L}+\mathrm{\&delta;}L$

${\mathrm{\&lambda;}}^{\&prime;}=\widetilde{\mathrm{\&lambda;}}+\mathrm{\&delta;}\mathrm{\&lambda;}$

Wherein,Being that SINS resolves obtained latitude, L ' is obtained accurate latitude after revising；Being that SINS resolves obtained latitude, λ ' is obtained accurate longitude after revising.