CN103591965A - Online calibrating method of ship-based rotary strapdown inertial navigation system - Google Patents

Abstract

The invention discloses an online calibrating method of a ship-based rotary strapdown inertial navigation system. The method comprises the following steps: establishing an inertial component output error model and an inertial navigation system error equation, and researching the calibration of inertial component parameter errors and determining the quantity of state and the quantity of measuration; determining the position and weight of a cubature point according to dimension of the quantity of state, deducing a state equation and a one-step state prediction and state prediction covariance matrix related to the cubature point, and introducing a multiple time-varying fading factor modified state prediction covariance matrix; and deducing a measuring equation related to the cubature point and the fading factors, a self-correlated covariance matrix, a cross-correlated covariance matrix, a gain matrix, a state estimated value and a state error covariance estimated value, and designing a strong tracking volume Kalman filtering method with strong tracking performance and strong robustness. The method disclosed by the invention estimates the inertial component parameter errors by a filtering algorithm and carries out online calibration and compensates the inertial component parameter errors, so that the navigation precision is effectively improved. The method has strong parameter-varying robustness.

Description

A kind of method of carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving

Technical field

The invention belongs to the technical field of carrier-borne rotary inertia guiding systems on-line proving field and filtering method application thereof, relate in particular to a kind of method of carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving.

Background technology

On-line proving technology is in inertial navigation system and the unseparated situation of carrier, thereby utilize certain external information to carry out the demarcation that Error Excitation carries out inertia device parameter error, at present, external the on-line proving technology of rotary strapdown inertial navitation system (SINS) being successfully applied in carrier-borne, missile-borne and airborne inertial navigation system, but carried out highly confidential to its correlation technique, along with the ripe and development of the rotary strapdown inertial navitation system (SINS) technology of China, the on-line proving technology of studying rotary strapdown inertial navitation system (SINS) is a current study hotspot.

And for carrier-borne rotary strapdown inertial navitation system (SINS), naval vessel is in actual motion environment time, owing to there is model state, simplify, the statistical property modeling of inertia device random noise and original state is inaccurate and take the normal value zero of inertia device partially as the model parameter of the representative problem such as undergo mutation causes the system model of inertial navigation system to certainly exist uncertainty, the state estimation value that system model uncertainty will certainly cause departs from the time of day of system, thereby cause that filtering accuracy reduces, even disperse, therefore, inertia device parameter error estimation problem is the difficult problem that carrier-borne rotary strapdown inertial navitation system (SINS) carries out not having in on-line proving technology solution.

Summary of the invention

The object of the embodiment of the present invention is to provide a kind of method of carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving, be intended to solve in carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving process, because the disturbing effects such as wave, sea wind exist, the uncertain situation of system model can cause filtering divergence, precision is low and the problem of poor robustness, has a strong impact on the problem of on-line proving precision.

The embodiment of the present invention is achieved in that a kind of method of carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving, and the method for this carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving comprises the following steps:

Step 1, sets up the output error models of gyro, accelerometer and the error equation of inertial navigation system;

Step 2, chosen position, speed and attitude error and inertia device parameter error, as filter status amount, are set up state equation;

Step 3, the difference of the position that the position that the inertial navigation system of usining resolves, speed, attitude information and GPS, Doppler log, star sensor provide, speed, attitude information, as wave filter measurement amount, is set up measurement equation;

Step 4, according to the quantity of state dimension n of wave filter and volume Kalman filtering algorithm characteristic, determines the position ξ that cubature is ordered _iwith corresponding weights ω _i, i=1 wherein ..., 2n;

Step 5, according to the cubature point obtaining, by Cholesky, decompose, the cubature point that obtains propagating in state equation, the position that system state equation is ordered due to cubature and the impact of weights can be propagated as another new state equation relevant to cubature point, obtain step status predication value and a status predication covariance matrix relevant to cubature point;

Step 6, adds one to become fading factor when multiple in the status predication covariance matrix in step 5;

Step 7, according to the cubature point obtaining in step 4, by Cholesky, decompose, the cubature point that obtains propagating in measurement equation, the position that measurement equation is ordered due to cubature and the impact of weights thereof can be propagated as another new measurement equation relevant with fading factor to cubature point;

Step 8, according to step 7, derive auto-correlation covariance matrix, simple crosscorrelation covariance matrix, kalman gain, state estimation value and the state error covariance estimated value relevant with fading factor to cubature point, realize the measurement of the strong tracking volume Kalman filtering algorithm with strong tracing property and strong robustness and upgrade;

Step 9, according to the concrete time of strong tracking volume Kalman filtering algorithm in step 5 and step 7, upgrade and measure renewal process quantity of state in step 3 is estimated, obtain inertia device parameters error amount, realized carrier-borne strapdown inertial navitation system (SINS) on-line proving.

Further, in step 4, the position ξ that cubature is ordered _iweights ω corresponding to it _i, i=1 ..., 2n is defined as:

${\mathrm{\ξ}}_i=\sqrt{n}{\left[1\right]}_i$

${\mathrm{\ω}}_i=\frac{1}{2n}$

Wherein, [1] _ithe i row that represent set [1], during for n=2, have $\left[1\right]=\{\left[\begin{array}{c}1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\end{array}\right],\left[\begin{array}{c}-1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\end{array}\right]\},i=1,...,2n.$

Further, in step 6, add one to become fading factor when multiple, in k+1 status predication covariance matrix constantly, add multiple time become fading factor λ _k+1be defined as:

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_0<1\end{array}\right.$

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}{N}_{k+1}}{\mathrm{tr}{M}_{k+1}}$

$N_{k+1}=C_{0,k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}$

$M_{k+1}=H_{k+1}{\mathrm{\&Phi;}}_{k+1}{P}_{k+1|k}{\mathrm{\&Phi;}}_{k+1}^T{H}_{k+1}^T$

In formula, λ _k+1>=1, trN _k+1and trM _k+1be respectively N _k+1and M _k+1mark, the transposition of superscript T representing matrix,

x _kfor k quantity of state constantly, for k predicted state amount constantly, h _k+1for describing the arbitrary function of measurement equation; Q _kfor the covariance matrix of system noise, R _k+1for the covariance matrix of measurement noise, P _k+1|kfor status predication covariance matrix, $C_{0,k+1}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_1{{\mathrm{\&gamma;}}_1}^T& k=0\\ \frac{{\mathrm{\&rho;}{C}_{0,k}+{\mathrm{\&gamma;}}_{k+1}{\mathrm{\&gamma;}}_{k+1}}^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1,\end{array}\right.$ Y _k+1for the quantity of state constantly of k+1 in new state equation, 0< ρ≤1 is forgetting factor, and status predication covariance matrix adds becomes fading factor λ when multiple _k+1after become:

$P_{k+1|k}^{\left(l\right)}={\mathrm{\&lambda;}}_{k+1}(\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T)+Q_k$

Wherein,

be a step status predication (utilizing k constantly to predict that k+1 constantly), ∑ is the symbol of suing for peace, and superscript (1) represents that parameter is subject to the impact of fading factor.

Further, in step 8, the auto-correlation covariance matrix obtaining is:

$P_{\mathrm{zz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left(\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T+R_{k+1}$

Wherein,

for the quantity of state in new state equation,

for the position that cubature is ordered, i=1 ..., 2n,

Simple crosscorrelation covariance matrix is:

$P_{\mathrm{xz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

The kalman gain relevant to fading factor, state estimation value and state error covariance estimated value, be respectively:

$K_{k+1}^{\left(l\right)}=P_{\mathrm{xz},k+1|k}^{\left(l\right)}{\left(P_{\mathrm{zz},k+1|k}^{\left(l\right)}\right)}^{-1}$

${\hat{x}}_{k+1|k+1}^{\left(l\right)}={\hat{x}}_{k+1|k}^{\left(l\right)}+K_{k+1}^{\left(l\right)}(z_{k+1}^{\left(l\right)}-{\hat{z}}_{k+1|k}^{\left(l\right)})$

$P_{k+1|k+1}^{\left(l\right)}=P_{k+1|k}^{\left(l\right)}-K_{k+1|k}^{\left(l\right)}{P}_{\mathrm{zz},k+1|k}^{\left(l\right)}{\left(K_{k+1|k}^{\left(l\right)}\right)}^T$

Wherein,

represent

contrary.

Further, the concrete steps of the method for this carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving are:

Step 1 needs the measured value of Real-time Collection gyro and accelerometer in the process of ship navigation, and the measured value of gyro is ${\hat{w}}^s={\left[\begin{array}{ccc}{w}_x^s& w_y^s& w_z^s\end{array}\right]}^T,$ The measured value of accelerometer is ${\hat{f}}^s={\left[\begin{array}{ccc}{f}_x^s& f_y^s& f_z^s\end{array}\right]}^T,$ Superscript s representative rotation system; Subscript x, y, z represents respectively the x of gyro and acceleration, y, tri-axles of z, preserve the measured value of GPS, Doppler log and star sensor in real time;

Step 2: under orientation large misalignment angle condition, inertial navigation system error equation exists significantly non-linear, sets up the inertial navigation system nonlinearity erron equation relevant to inertia device parameter error;

Step 3: the main object of carrier-borne rotary inertia guiding systems on-line proving is inertia device scale factor error and constant error item, according to the qualitative question marked to inertia device parameter error, choose naval vessel site error, velocity error, attitude error, all gyroscope constant value drifts and gyro scale factor error, horizontal accelerometer zero partially and accelerometer scale factor error as filter status amount;

Step 5: according to quantity of state dimension n and volume Kalman filtering algorithm characteristic, obtain cubature point position and weights and be respectively:

${\mathrm{\&xi;}}_i=\sqrt{n}{\left[1\right]}_i$

${\mathrm{\&omega;}}_i=\frac{1}{2n}$

Wherein, n is quantity of state dimension, i=1 ..., 2n, [1] _ithe i row that represent set [1], during for n=2, $\left[1\right]=\{\left[\begin{array}{c}1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\end{array}\right],\left[\begin{array}{c}-1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\end{array}\right]\};$

Step 6: according to the cubature point obtaining in step 5, by Cholesky, decompose, the cubature point that obtains propagating in state equation, the position that system state equation is ordered due to cubature and the impact of weights can be propagated as another new state equation relevant to cubature point, obtain step status predication value and a status predication covariance matrix relevant to cubature point;

Step 7: add one to become fading factor when multiple and make it have the uncertain robustness of answering system model in the status predication covariance matrix in step 6:

In order to make wave filter there is the uncertain robustness of answering system model, in status predication covariance matrix, introduce fading factor λ _k+1:

$P_{k+1|k}^{\left(l\right)}={\mathrm{\&lambda;}}_{k+1}(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T)+Q_k$

This fading factor is determined by following formula:

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_0<1\end{array}\right.$

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}{N}_{k+1}}{\mathrm{tr}{M}_{k+1}}$

$N_{k+1}=C_{0,k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}$

$M_{k+1}=H_{k+1}{\mathrm{\&Phi;}}_{k+1}{P}_{k+1|k}{\mathrm{\&Phi;}}_{k+1}^T{H}_{k+1}^T$

Wherein, λ _k+1>=1, trN _k+1and trM _k+1be respectively N _k+1and M _k+1mark,

for H _k+1transposition, for Φ _k+1transposition,

x _kfor k quantity of state constantly,

for k predicted state amount constantly, h _k+1for describing the arbitrary function of measurement equation; Q _kfor the covariance matrix of system noise, R _k+1for the covariance matrix of measurement noise, P _k+1|kfor status predication covariance matrix,

Meanwhile, C _{0, k}must meet:

$C_{0,k+1}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_1{{\mathrm{\&gamma;}}_1}^T& k=0\\ \frac{{\mathrm{\&rho;}{C}_{0,k}+{\mathrm{\&gamma;}}_{k+1}{\mathrm{\&gamma;}}_{k+1}}^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.$

Wherein, γ _l+1for the quantity of state constantly of k+1 in new state equation, 0< ρ≤1 is forgetting factor, and value is 0.95 conventionally;

Step 8: according to the cubature point obtaining in step 5, by Cholesky, decompose, the cubature point that obtains propagating in measurement equation, the position that system measurements equation is ordered due to cubature and the impact of weights thereof can be propagated as another new measurement equation relevant with fading factor to cubature point;

Step 9: derive volume Kalman filtering algorithm according to step 8 and measure auto-correlation covariance matrix and the simple crosscorrelation covariance matrix relevant with fading factor to cubature point in renewal, can obtain the kalman gain relevant with fading factor to cubature point, state estimation value and state error covariance estimated value, realize the renewal of the strong tracking volume Kalman filtering algorithm with strong tracking performance:

According to the algorithm of kalman gain, state estimation value and state error covariance estimated value in above filtering update algorithm and volume Kalman filtering algorithm, kalman gain that can be relevant to fading factor, state estimation value and state error covariance estimated value;

Step 10: upgrade and measure renewal process according to the concrete time of strong tracking volume Kalman filtering algorithm in step 6 and step 8 the quantity of state in step 3 is estimated, obtain position, speed and attitude error and the inertia device parameters error of inertial navigation system.

Further, in step 2, the angular velocity error ε of gyro output ^sspecific force error delta f with accelerometer output ^sbe respectively:

${\mathrm{\&epsiv;}}^s=D_0+{\mathrm{\&Delta;S}}_g{\hat{w}}^s$

${\mathrm{\&Delta;f}}^s=A_0+{\mathrm{\&Delta;S}}_a{\hat{f}}^s$

Wherein, Δ S _g=diag[Δ S _gxΔ S _gyΔ S _gx] be the scale factor error of gyro, D ₀=[D _0xd _0yd _0z] ^tfor the constant value drift of gyro, Δ S _a=diag[Δ S _axΔ S _ayΔ S _az] be accelerometer scale factor error, A ₀=[A _0xa _0ya _0z] ^tfor accelerometer bias, subscript g represents gyro, and a represents accelerometer;

Under orientation large misalignment angle condition, attitude error equations is:

$\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}=C_w^{-1}[(I-C_n^{n^{\&prime;}}){\hat{w}}_{\mathrm{in}}^n+C_n^{n^{\&prime;}}{\mathrm{\&delta;w}}_{\mathrm{in}}^n{-C}_b^{n^{\&prime;}}({\mathrm{\&epsiv;}}^b+w_g^b)]$

Wherein, α=[α _eα _nα _u] ^tfor attitude error angle, subscript e, n, u represent respectively sky, northeast, and α is

derivative, subscript n representative navigation system, i represents inertial system, n ' representative is calculated navigation and is,

for the angle of rotation speed of navigation system with respect to inertial system, sin, cos are just being respectively, cosine function, $C_w^{-1}=\frac{1}{\cos {\mathrm{\&alpha;}}_x}\left[\begin{array}{ccc}\cos {\mathrm{\&alpha;}}_x\cos {\mathrm{\&alpha;}}_y& 0& \cos {\mathrm{\&alpha;}}_x\sin {\mathrm{\&alpha;}}_y\\ \sin {\mathrm{\&alpha;}}_x\sin {\mathrm{\&alpha;}}_y& \cos {\mathrm{\&alpha;}}_x& -\cos {\mathrm{\&alpha;}}_y\sin {\mathrm{\&alpha;}}_x\\ -\sin {\mathrm{\&alpha;}}_y& 0& \cos {\mathrm{\&alpha;}}_y\end{array}\right],$

for the error of calculation, for the strapdown matrix calculating,

for navigation is tied to the transition matrix that calculates navigation system,

for rotation is tied to the transition matrix that carrier is,

for zero-mean white Gaussian noise;

Under orientation large misalignment angle condition, velocity error equation is:

$\begin{array}{c}{\mathrm{\&delta;}\stackrel{\&CenterDot;}{v}}^n=(I-C_{n^{\&prime;}}^n)C_b^{n^{\&prime;}}{f}^b-(2\mathrm{\&delta;}{w}_{\mathrm{ie}}^n+{\mathrm{\&delta;w}}_{\mathrm{en}}^n)\&times;({\hat{v}}^n-{\mathrm{\&delta;v}}^n)-({2\hat{w}}_{\mathrm{ie}}^n+{\hat{w}}_{\mathrm{en}}^n)\&times;{\mathrm{\&delta;v}}^n+{\mathrm{\&delta;g}}^n\\ +C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}({\mathrm{\&Delta;f}}^b+w_a^b)\end{array}$

Wherein, δ v ⁿ=[δ v _eδ v _n] ^tfor horizontal velocity error,

for δ v ⁿderivative,

for calculating navigation, be tied to the transition matrix of navigation system,

for carrier is tied to the transition matrix that navigation is, f ^bthe projection of fastening at carrier for accelerometer specific force,

for earth rotation angular speed is in the value of calculating navigation system,

for the value of navigating and being is being calculated with respect to the angle of rotation speed of earth system by the system of navigating,

with

for

the error of calculation,

for the speed of navigation calculation,

for the measured deviation of accelerometer, δ g ⁿfor the error of calculation of gravitational vector,

zero-mean white Gaussian noise;

Under orientation large misalignment angle condition, the site error of carrier is unaffected, and site error equation is:

$\left\{\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\frac{{\mathrm{\&delta;v}}_n}{R}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{{\mathrm{\&delta;v}}_e}{R}\sec L+\frac{v_n}{R}\tan L\sec \mathrm{L\&delta;L}\end{array}\right.$

Wherein,

be respectively the derivative of longitude, latitude error, δ L is longitude error, and L is longitude, and R is earth radius, and sec is secant, and tan is tan.

Further, in step 4, suppose that the system filter equation of carrier-borne rotary inertial navigation is:

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

Wherein: x _kfor k quantity of state constantly, z _kfor k measurement amount constantly, f() and h() be respectively system nonlinear state function and measure function; u _kand v _kfor zero-mean white Gaussian noise;

The quantity of state that is located at strapdown inertial navitation system (SINS) filtering equations in line calibration technique is as follows:

x(t)=[δL?δλ?δv _e?δv _n?α _e?α _n?α _u?D _0x?D _0y?D _0z?A _0x?A _0y?ΔS _gx?ΔS _gy?ΔS _gz?ΔS _ax?ΔS _ay] ^T，

Utilize GPS, Doppler log and star sensor external unit to provide high precision reference information for carrier-borne rotary strapdown inertial navitation system (SINS), do poor comparison with position, speed, attitude that inertial navigation system provides, and as the measurement amount of system filter device, amount is measured as:

z(t)=[L _INS-Lλ _INS-λv _eINS-v _ev _nINS-v _Nα _eINS-α _eα _nINS-α _nα _uINS-α _u] ^T

Wherein, first position, speed and attitude information that is respectively inertial reference calculation of each element in measurement amount, second is respectively high precision position, speed and the attitude information that extraneous reference device provides.

Further, in step 6, the position of ordering according to cubature and weights, system state equation is propagated and is:

γ _i，k+1|k＝f _k(ξ _i，k，u _k)+q _k，i=0，1，…，2n

Wherein, γ _{i, k+1|k}for the quantity of state in new state equation, q _kfor system noise,

In conjunction with volume Kalman filtering algorithm, can obtain a step status predication is:

${\hat{x}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{f}_k({\mathrm{\&xi;}}_{i,k},u_k)+q_k$

Status predication covariance matrix is:

$P_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T+Q_k$

Wherein, u _kn dimension control inputs vector, Q _kcovariance matrix for system noise.

Further, in step 8, new measurement equation is:

Utilize Cholesky to decompose

have:

$P_{k+1|k}^{\left(l\right)}=S_{k+1|k}{S}_{k+1|k}^T$

Wherein, S _k+1|kfor Spherical integration,

Cubature point that can be relevant to fading factor, for:

${\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}=S_{k+1|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k}$

The cubature point of propagating by measurement equation is:

${\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}=h_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

, system k+1 measurement predictor is constantly:

${\hat{z}}_{k+1|k}^{\left(l\right)}=\frac{1}{2}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

Auto-correlation covariance matrix is:

${P_{\mathrm{zz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T{-\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\left(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T+R_{k+1}$

Wherein, R _k+1for the covariance matrix of measurement noise,

Simple crosscorrelation covariance matrix is:

${P_{\mathrm{xz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right).$

Further, in step 9, the kalman gain that fading factor is relevant, state estimation value and state error covariance estimated value, be respectively:

${K_{k+1}^{\left(l\right)}=P_{\mathrm{xz},k+1|k}^{\left(l\right)}\left(P_{\mathrm{zz},k+1|k}^{\left(l\right)}\right)}^{-1}$

${\hat{x}}_{k+1|k+1}^{\left(l\right)}={\hat{x}}_{k+1|k}^{\left(l\right)}+K_{k+1}^{\left(l\right)}(z_{k+1}^{\left(l\right)}-{\hat{z}}_{k+1|k}^{\left(l\right)})$

${P_{k+1|k+1}^{\left(l\right)}=P_{k+1|k}^{\left(l\right)}-K_{k+1|k}^{\left(l\right)}{P}_{\mathrm{zz},k+1|k}^{\left(l\right)}\left(K_{k+1|k}^{\left(l\right)}\right)}^T.$

The method of carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving provided by the invention, by utilizing strong tracking volume kalman filter method to carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving, realized the on-line proving of inertial navigation system, compensate inertia device parameter error, improved inertial navigation system navigation accuracy.The present invention has kept the strong tracking power to mutation status, there is the stronger robustness about the change of real system parameter, estimated accuracy is high, without linearization, process calculate simple, computing time is short and can process exactly higher-dimension number system, be difficult for dispersing, under complicated marine environment, effectively improve system navigation accuracy.

Accompanying drawing explanation

Fig. 1 is the method flow diagram of the carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving that provides of the embodiment of the present invention;

Fig. 2 is the strong tracking volume Kalman filtering process flow diagram that the embodiment of the present invention provides.

Embodiment

In order to make object of the present invention, technical scheme and advantage clearer, below in conjunction with embodiment, the present invention is further elaborated.Should be appreciated that specific embodiment described herein, only in order to explain the present invention, is not intended to limit the present invention.

Fig. 1 shows the method flow of carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving provided by the invention.For convenience of explanation, only show part related to the present invention.

The method of carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving, the method for this carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving comprises the following steps:

Step 1, sets up the output error models of gyro, accelerometer and the error equation of inertial navigation system;

Step 2, chosen position, speed and attitude error and inertia device parameter error, as filter status amount, are set up state equation;

Step 3, the difference of the position that the position that the inertial navigation system of usining resolves, speed, attitude information and GPS, Doppler log, star sensor provide, speed, attitude information, as wave filter measurement amount, is set up measurement equation;

Step 4, according to the quantity of state dimension n of wave filter and volume Kalman filtering algorithm characteristic, determines the position ξ that cubature is ordered _iwith corresponding weights ω _i, i=1 wherein ..., 2n;

Step 5, according to the cubature point obtaining, by Cholesky, decompose, the cubature point that obtains propagating in state equation, the position that system state equation is ordered due to cubature and the impact of weights can be propagated as another new state equation relevant to cubature point, obtain step status predication value and a status predication covariance matrix relevant to cubature point;

Step 6, adds one to become fading factor when multiple in the status predication covariance matrix in step 5;

Step 7, according to the cubature point obtaining in step 4, by Cholesky, decompose, the cubature point that obtains propagating in measurement equation, the position that measurement equation is ordered due to cubature and the impact of weights thereof can be propagated as another new measurement equation relevant with fading factor to cubature point;

Step 8, according to step 7, derive auto-correlation covariance matrix, simple crosscorrelation covariance matrix, kalman gain, state estimation value and the state error covariance estimated value relevant with fading factor to cubature point, realize the measurement of the strong tracking volume Kalman filtering algorithm with strong tracing property and strong robustness and upgrade;

Step 9, according to the concrete time of strong tracking volume Kalman filtering algorithm in step 5 and step 7, upgrade and measure renewal process quantity of state in step 3 is estimated, obtain inertia device parameters error amount, realized carrier-borne strapdown inertial navitation system (SINS) on-line proving.

As a prioritization scheme of the embodiment of the present invention, in step 4, the position ξ that cubature is ordered _iweights ω corresponding to it _i, i=1 ..., 2n is defined as:

${\mathrm{\&xi;}}_i=\sqrt{n}{\left[1\right]}_i$

${\mathrm{\&omega;}}_i=\frac{1}{2n}$

Wherein, [1] _ithe i row that represent set [1], during for n=2, have $\left[1\right]=\{\left[\begin{array}{c}1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\end{array}\right],\left[\begin{array}{c}-1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\end{array}\right]\},$ i＝1，…，2n。

As a prioritization scheme of the embodiment of the present invention, in step 6, add one to become fading factor when multiple, in k+1 status predication covariance matrix constantly, add multiple time become fading factor λ _k+1be defined as:

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_0<1\end{array}\right.$

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}{N}_{k+1}}{\mathrm{tr}{M}_{k+1}}$

$N_{k+1}=C_{0,k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}$

$M_{k+1}=H_{k+1}{\mathrm{\&Phi;}}_{k+1}{P}_{k+1|k}{\mathrm{\&Phi;}}_{k+1}^T{H}_{k+1}^T$

In formula, λ _k+1>=1, trN _k+1and trM _k+1be respectively N _k+1and M _k+1mark, the transposition of superscript T representing matrix,

x _kfor k quantity of state constantly,

for k predicted state amount constantly, h _k+1for describing the arbitrary function of measurement equation; Q _kfor the covariance matrix of system noise, R _k+1for the covariance matrix of measurement noise, P _k+1|kfor status predication covariance matrix, $C_{0,k+1}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_1{{\mathrm{\&gamma;}}_1}^T& k=0\\ \frac{\mathrm{\&rho;}{C}_{0,k}+{\mathrm{\&gamma;}}_{k+1}{{\mathrm{\&gamma;}}_{k+1}}^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.,$ γ _k+1for the quantity of state constantly of k+1 in new state equation, 0< ρ≤1 is forgetting factor, and status predication covariance matrix adds becomes fading factor λ when multiple _k+1after become:

$P_{k+1|k}^{\left(l\right)}={\mathrm{\&lambda;}}_{k+1}(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T)+Q_k$

Wherein,

be a step status predication (utilizing k constantly to predict that k+1 constantly), ∑ is the symbol of suing for peace, and superscript (1) represents that parameter is subject to the impact of fading factor.

As a prioritization scheme of the embodiment of the present invention, in step 8, the auto-correlation covariance matrix obtaining is:

$P_{\mathrm{zz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&chi;}}_{i.k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i.k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T+R_{k+1}$

Wherein,

for the quantity of state in new state equation,

for the position that cubature is ordered, i=1 ..., 2n,

Simple crosscorrelation covariance matrix is:

$P_{\mathrm{xz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

The kalman gain relevant to fading factor, state estimation value and state error covariance estimated value, be respectively:

$K_{k+1}^{\left(l\right)}=P_{\mathrm{xz}.k+1|k}^{\left(l\right)}{\left(P_{\mathrm{zz},k+1|k}^{\left(l\right)}\right)}^{-1}$

${\hat{x}}_{k+1|k+1}^{\left(l\right)}={\hat{x}}_{k+1|k}^{\left(l\right)}+K_{k+1}^{\left(l\right)}(z_{k+1}^{\left(l\right)}-{\hat{z}}_{k+1|k}^{\left(l\right)})$

$P_{k+1|k+1}^{\left(l\right)}=P_{k+1|k}^{\left(l\right)}-K_{k+1|k}^{\left(l\right)}{P_{\mathrm{zz},k+1|k}^{\left(l\right)}\left(K_{k+1|k}^{\left(l\right)}\right)}^T$

Wherein, represent

contrary.

As a prioritization scheme of the embodiment of the present invention, the concrete steps of the method for this carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving are:

Step 1 needs the measured value of Real-time Collection gyro and accelerometer in the process of ship navigation, and the measured value of gyro is ${\hat{w}}^s={\left[\begin{array}{ccc}{w}_x^s& w_y^s& w_z^s\end{array}\right]}^T,$ The measured value of accelerometer is ${\hat{f}}^s={\left[\begin{array}{ccc}{f}_x^s& f_y^s& f_z^s\end{array}\right]}^T,$ Superscript s representative rotation system; Subscript x, y, z represents respectively the x of gyro and acceleration, y, tri-axles of z, preserve the measured value of GPS, Doppler log and star sensor in real time;

Step 2: under orientation large misalignment angle condition, inertial navigation system error equation exists significantly non-linear, sets up the inertial navigation system nonlinearity erron equation relevant to inertia device parameter error;

Step 3: the main object of carrier-borne rotary inertia guiding systems on-line proving is inertia device scale factor error and constant error item, according to the qualitative question marked to inertia device parameter error, choose naval vessel site error, velocity error, attitude error, all gyroscope constant value drifts and gyro scale factor error, horizontal accelerometer zero partially and accelerometer scale factor error as filter status amount;

Step 5: according to quantity of state dimension n and volume Kalman filtering algorithm characteristic, obtain cubature point position and weights and be respectively:

${\mathrm{\&xi;}}_i=\sqrt{n}{\left[1\right]}_i$

${\mathrm{\&omega;}}_i=\frac{1}{2n}$

Wherein, n is quantity of state dimension, i=1 ..., 2n, [1] _ithe i row that represent set [1], during for n=2, $\left[1\right]=\{\left[\begin{array}{c}1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\end{array}\right],\left[\begin{array}{c}-1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\end{array}\right]\};$

Step 6: according to the cubature point obtaining in step 5, by Cholesky, decompose, the cubature point that obtains propagating in state equation, the position that system state equation is ordered due to cubature and the impact of weights can be propagated as another new state equation relevant to cubature point, obtain step status predication value and a status predication covariance matrix relevant to cubature point;

Step 7: add one to become fading factor when multiple and make it have the uncertain robustness of answering system model in the status predication covariance matrix in step 6:

In order to make wave filter there is the uncertain robustness of answering system model, in status predication covariance matrix, introduce fading factor λ _k+1:

$P_{k+1|k}^{\left(l\right)}={\mathrm{\&lambda;}}_{k+1}(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T)+Q_k$

This fading factor is determined by following formula:

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_0<1\end{array}\right.$

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}{N}_{k+1}}{\mathrm{tr}{M}_{k+1}}$

$N_{k+1}=C_{0,k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}$

$M_{k+1}=H_{k+1}{\mathrm{\&Phi;}}_{k+1}{P}_{k+1|k}{\mathrm{\&Phi;}}_{k+1}^T{H}_{k+1}^T$

Wherein, λ _k+1>=1, trN _k+1and trM _k+1be respectively N _k+1and M _k+1mark,

for H _k+1transposition, for Φ _k+1transposition,

x _kfor k quantity of state constantly,

for k predicted state amount constantly, h _k+1for describing the arbitrary function of measurement equation; Q _kfor the covariance matrix of system noise, R _k+1for the covariance matrix of measurement noise, P _k+1|kfor status predication covariance matrix,

Meanwhile, C _{0, k}must meet:

$C_{0,k+1}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_1{{\mathrm{\&gamma;}}_1}^T& k=0\\ \frac{\mathrm{\&rho;}{C}_{0,k}+{\mathrm{\&gamma;}}_{k+1}{{\mathrm{\&gamma;}}_{k+1}}^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.$

Wherein, γ _k+1for the quantity of state constantly of k+1 in new state equation, 0< ρ≤1 is forgetting factor, and value is 0.95 conventionally;

Step 8: according to the cubature point obtaining in step 5, by Cholesky, decompose, the cubature point that obtains propagating in measurement equation, the position that system measurements equation is ordered due to cubature and the impact of weights thereof can be propagated as another new measurement equation relevant with fading factor to cubature point;

Step 9: derive volume Kalman filtering algorithm according to step 8 and measure auto-correlation covariance matrix and the simple crosscorrelation covariance matrix relevant with fading factor to cubature point in renewal, can obtain the kalman gain relevant with fading factor to cubature point, state estimation value and state error covariance estimated value, realize the renewal of the strong tracking volume Kalman filtering algorithm with strong tracking performance:

According to the algorithm of kalman gain, state estimation value and state error covariance estimated value in above filtering update algorithm and volume Kalman filtering algorithm, kalman gain that can be relevant to fading factor, state estimation value and state error covariance estimated value;

Step 10: upgrade and measure renewal process according to the concrete time of strong tracking volume Kalman filtering algorithm in step 6 and step 8 the quantity of state in step 3 is estimated, obtain position, speed and attitude error and the inertia device parameters error of inertial navigation system.

As a prioritization scheme of the embodiment of the present invention, in step 2, the angular velocity error ε of gyro output ^sspecific force error delta f with accelerometer output ^sbe respectively:

${\mathrm{\&epsiv;}}^s=D_0+\mathrm{\&Delta;}{S}_g{\hat{w}}^s$

$\mathrm{\&Delta;}{f}^s=A_0+\mathrm{\&Delta;}{S}_a{\hat{f}}^s$

Wherein, Δ S _g=diag[Δ S _gxΔ S _gyΔ S _gz] be the scale factor error of gyro, D ₀=[D _0xd _0yd _0z] ^tfor the constant value drift of gyro, Δ S _a=diag[Δ S _axΔ S _ayΔ S _az] be accelerometer scale factor error, A ₀=[A _0xa _0ya _0z] ^tfor accelerometer bias, subscript g represents gyro, and a represents accelerometer;

Under orientation large misalignment angle condition, attitude error equations is:

$\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}=C_w^{-1}[(I-C_n^{n^{\text{'}}}){\hat{w}}_{\mathrm{in}}^n+C_n^{n^{\text{'}}}\mathrm{\&delta;}{w}_{\mathrm{in}}^n-C_b^{n^{\text{'}}}({\mathrm{\&epsiv;}}^b+w_g^b)]$

Wherein, α=[α _eα _nα _u] ^tfor attitude error angle, subscript e, n, u represent respectively sky, northeast, and α is

derivative, subscript n representative navigation system, i represents inertial system, n ' representative is calculated navigation and is,

for the angle of rotation speed of navigation system with respect to inertial system, sin, cos are just being respectively, cosine function, $C_w^{-1}=\frac{1}{\cos {\mathrm{\&alpha;}}_x}\left[\begin{array}{ccc}\cos {\mathrm{\&alpha;}}_x\cos {\mathrm{\&alpha;}}_y& 0& \cos {\mathrm{\&alpha;}}_x\sin {\mathrm{\&alpha;}}_y\\ \sin {\mathrm{\&alpha;}}_x\sin {\mathrm{\&alpha;}}_y& \cos {\mathrm{\&alpha;}}_x& -\cos {\mathrm{\&alpha;}}_y\sin {\mathrm{\&alpha;}}_x\\ -\sin {\mathrm{\&alpha;}}_y& 0& \cos {\mathrm{\&alpha;}}_y\end{array}\right],$

for

the error of calculation,

for the strapdown matrix calculating,

for navigation is tied to the transition matrix that calculates navigation system,

for rotation is tied to the transition matrix that carrier is,

for zero-mean white Gaussian noise;

Under orientation large misalignment angle condition, velocity error equation is:

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}^n=(I-C_{n^{\text{'}}}^n)C_b^{n^{\text{'}}}{f}^b-(2\mathrm{\&delta;}{w}_{\mathrm{ie}}^n+\mathrm{\&delta;}{w}_{\mathrm{en}}^n)\&times;({\hat{v}}^n-\mathrm{\&delta;}{v}^n)-(2{\hat{w}}_{\mathrm{ie}}^n+{\hat{w}}_{\mathrm{en}}^n)\&times;\mathrm{\&delta;}{v}^n+\mathrm{\&delta;}{g}^n\\ +C_{n^{\text{'}}}^n{C}_b^{n^{\text{'}}}(\mathrm{\&Delta;}{f}^b+w_a^b)\end{array}$

Wherein, δ v ⁿ=[δ v _eδ v _n] ^tfor horizontal velocity error,

for δ v ⁿderivative,

for calculating navigation, be tied to the transition matrix of navigation system,

for carrier is tied to the transition matrix that navigation is, f ^bthe projection of fastening at carrier for accelerometer specific force,

for earth rotation angular speed is in the value of calculating navigation system,

for the value of navigating and being is being calculated with respect to the angle of rotation speed of earth system by the system of navigating,

with

for

the error of calculation, for the speed of navigation calculation,

for the measured deviation of accelerometer, δ g ⁿfor the error of calculation of gravitational vector,

zero-mean white Gaussian noise;

Under orientation large misalignment angle condition, the site error of carrier is unaffected, and site error equation is:

$\left\{\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\frac{\mathrm{\&delta;}{v}_n}{R}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{{\mathrm{\&delta;v}}_e}{R}\sec L+\frac{v_n}{R}\tan L\sec \mathrm{L\&delta;L}\end{array}\right.$

Wherein,

be respectively the derivative of longitude, latitude error, δ L is longitude error, and L is longitude, and R is earth radius, and sec is secant, and tan is tan.

As a prioritization scheme of the embodiment of the present invention, in step 4, suppose that the system filter equation of carrier-borne rotary inertial navigation is:

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

Wherein: x _kfor k quantity of state constantly, z _kfor k measurement amount constantly, f () and h () are respectively system nonlinear state function and measure function; u _kand v _kfor zero-mean white Gaussian noise;

The quantity of state that is located at strapdown inertial navitation system (SINS) filtering equations in line calibration technique is as follows:

X (t)=[δ L δ λ δ v _eδ v _nα _eα _nα _ud _0xd _0yd _0za _0xa _0yΔ S _gxΔ S _gyΔ S _gzΔ S _axΔ S _ay] ^tutilize GPS, Doppler log and star sensor external unit to provide high precision reference information for carrier-borne rotary strapdown inertial navitation system (SINS), position, speed, the attitude providing with inertial navigation system done relatively poor, and as the measurement amount of system filter device, amount is measured as:

Z (t)=[L _iNS-L λ _iNS-λ v _eINS-v _ev _nINS-v _nα _eINS-α _eα _nINS-α _nα _uINS-α _u] ^twherein, first position, speed and attitude information that is respectively inertial reference calculation of each element in measurement amount, second is respectively high precision position, speed and the attitude information that extraneous reference device provides.

As a prioritization scheme of the embodiment of the present invention, in step 6, the position of ordering according to cubature and weights, system state equation is propagated and is:

γ _i，k+1|k=f _k(ξ _i，k，u _k)+q _k，i=0，1，...，2n

Wherein, γ _{i, k+1|k}for the quantity of state in new state equation, q _kfor system noise,

In conjunction with volume Kalman filtering algorithm, can obtain a step status predication is:

${\hat{x}}_{k+1|k}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{f}_k({\mathrm{\&xi;}}_{i,k},u_k)+q_k$

Status predication covariance matrix is:

$P_{k+1|k}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T+Q_k$

Wherein, u _kn dimension control inputs vector, Q _kcovariance matrix for system noise.

As a prioritization scheme of the embodiment of the present invention, in step 8, new measurement equation is:

Utilize Cholesky to decompose

have:

$P_{k+1|k}^{\left(l\right)}=S_{k+1|k}{S}_{k+1|k}^T$

Wherein, S _k+1|kfor Spherical integration,

Cubature point that can be relevant to fading factor, for:

${\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}=S_{k+1|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k}$

The cubature point of propagating by measurement equation is:

$x_{i,k+1|k}^{\left(l\right)}=h_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

, system k+1 measurement predictor is constantly:

${\hat{z}}_{k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

Auto-correlation covariance matrix is:

$P_{\mathrm{zz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{x}_{i,k+1|k}^{\left(l\right)}{\left(x_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T+R_{k+1}$

Wherein, R _k+1for the covariance matrix of measurement noise,

Simple crosscorrelation covariance matrix is:

$P_{\mathrm{xz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left(x_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right).$

As a prioritization scheme of the embodiment of the present invention, in step 9, the kalman gain that fading factor is relevant, state estimation value and state error covariance estimated value, be respectively:

$K_{k+1}^{\left(l\right)}=P_{\mathrm{xz},k+1|k}^{\left(l\right)}{\left(P_{\mathrm{zz},k+1|k}^{\left(l\right)}\right)}^{-1}$

${\hat{x}}_{k+1|k+1}^{\left(l\right)}={\hat{x}}_{k+1|k}^{\left(l\right)}+K_{k+1}^{\left(l\right)}(z_{k+1}^{\left(l\right)}-{\hat{z}}_{k+1|k}^{\left(l\right)})$

$P_{k+1|k+1}^{\left(l\right)}=P_{k+1|k}^{\left(l\right)}-K_{k+1|k}^{\left(l\right)}{P}_{\mathrm{zz},k+1|k}^{\left(l\right)}{\left(K_{k+1|k}^{\left(l\right)}\right)}^T.$

Below in conjunction with drawings and the specific embodiments, application principle of the present invention is further described.

As shown in Figure 1, the method for the carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving of the embodiment of the present invention comprises the following steps:

S101: set up the output error models of gyro, accelerometer and the error equation of inertial navigation system;

S102: according to step S101, the qualitative question marked of inertia device parameter error is studied, chosen position, speed and attitude error and inertia device parameter error, as filter status amount, are set up state equation;

S103: the difference of the position that the position that the inertial navigation system of usining resolves, speed, attitude information and GPS, Doppler log, star sensor provide, speed, attitude information, as wave filter measurement amount, is set up measurement equation;

S104: according to the quantity of state dimension of wave filter and volume Kalman filtering algorithm characteristic, determine the position weights corresponding to it that cubature is ordered, i=1 wherein ..., 2n;

S105: according to the cubature point obtaining in step S104, by Cholesky, decompose, the cubature point that can obtain propagating in state equation, the position that system state equation is ordered due to cubature and the impact of weights thereof can be propagated as another new state equation relevant to cubature point, and then can obtain step status predication value and a status predication covariance matrix relevant to cubature point;

S106: add one to become fading factor when multiple in the status predication covariance matrix in step S105;

S107: according to the cubature point obtaining in step S104, by Cholesky, decompose, the cubature point that obtains propagating in measurement equation, the position that measurement equation is ordered due to cubature and the impact of weights thereof can be propagated as another new measurement equation relevant with fading factor to cubature point;

S108: derive auto-correlation covariance matrix, simple crosscorrelation covariance matrix, kalman gain, state estimation value and the state error covariance estimated value relevant with fading factor to cubature point according to step S107, realize the measurement of the strong tracking volume Kalman filtering algorithm with strong tracing property and strong robustness and upgrade;

S109: with reference to the filtering process flow diagram of figure 2, according to the concrete time of strong tracking volume Kalman filtering algorithm in step S105 and step S107, upgrade and measure renewal process quantity of state in step S103 is estimated, obtain inertia device (gyro and accelerometer) parameters error amount, realized carrier-borne strapdown inertial navitation system (SINS) on-line proving;

Specific embodiments of the invention:

In carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving process, because the disturbing effects such as wave, sea wind exist, the uncertain situation of system model can cause filtering divergence, precision is low and the problem of poor robustness, have a strong impact on on-line proving precision, and the proposition of following the tracks of by force volume Kalman filtering can effectively overcome the above problems, therefore, the research of the carrier-borne rotary strapdown inertial navitation system (SINS) online calibration method based on strong tracking volume Kalman filtering has great importance

The present invention includes the following step:

Step 1: consider that carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving quantity of state to be estimated is more, system dimension is larger, data are carried out processing in real time the capabilities limits that is subject to navigational computer, therefore, in the process of ship navigation, need the measured value of Real-time Collection gyro and accelerometer, wherein, the measured value of gyro is ${\hat{w}}^s={\left[\begin{array}{ccc}{w}_x^s& w_y^s& w_z^s\end{array}\right]}^T,$ The measured value of accelerometer is ${\hat{f}}^s={\left[\begin{array}{ccc}{f}_x^s& f_y^s& f_z^s\end{array}\right]}^T,$ Superscript s representative rotation system; Subscript x, y, z represents respectively the x of gyro and acceleration, y, tri-axles of z; And need to preserve in real time the measured value of GPS, Doppler log and star sensor;

Step 2: naval vessel rides the sea and is subject to the impact of the factors such as stormy waves, ocean current and seawater resistance in process and easily produces rocking by a relatively large margin, strapdown inertial navitation system (SINS) exists compared with large misalignment angle, under orientation large misalignment angle condition, inertial navigation system error equation exists significantly non-linear, sets up the inertial navigation system nonlinearity erron equation relevant to inertia device parameter error;

For carrier-borne rotary strapdown inertial navitation system (SINS), at periodic calibrating, in the cycle, can think that the alignment error of system does not change, the main object of demarcation is: gyro scale factor error, gyroscope constant value drift, accelerometer scale factor error and accelerometer bias;

The angular velocity error ε of gyro output ^sspecific force error delta f with accelerometer output ^sbe respectively:

${\mathrm{\&epsiv;}}^s=D_0+\mathrm{\&Delta;}{S}_g{\hat{w}}^s$

$\mathrm{\&Delta;}{f}^s=A_0+\mathrm{\&Delta;}{S}_a{\hat{f}}^s$

Wherein, Δ S _g=diag[Δ S _gxΔ S _gyΔ S _gx] be the scale factor error of gyro, D ₀=[D _0xd _0yd _0z] ^tfor the constant value drift of gyro, Δ S _a=diag[Δ S _axΔ S _ayΔ S _az] be accelerometer scale factor error, A ₀=[A _0xa _0ya _0z] ^tfor accelerometer bias, subscript g represents gyro, and a represents accelerometer;

Under orientation large misalignment angle condition, attitude error equations is:

$\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}=C_w^{-1}[(I-C_n^{n^{\&prime;}}){\hat{w}}_{\mathrm{in}}^n+C_n^{n^{\&prime;}}\mathrm{\&delta;}{w}_{\mathrm{in}}^n-C_b^{n^{\&prime;}}({\mathrm{\&epsiv;}}^b+w_g^b)]$

Wherein, α=[α _eα _nα _u] ^tfor attitude error angle, subscript e, n, u represent respectively sky, northeast, and a is

derivative, subscript n representative navigation system, i represents inertial system, n ' representative is calculated navigation and is,

for the angle of rotation speed of navigation system with respect to inertial system, sin, cos are just being respectively, cosine function, $C_w^{-1}=\frac{1}{\cos {\mathrm{\&alpha;}}_x}\left[\begin{array}{ccc}\cos {\mathrm{\&alpha;}}_x\cos {\mathrm{\&alpha;}}_y& 0& \cos {\mathrm{\&alpha;}}_x\sin {\mathrm{\&alpha;}}_y\\ \sin {\mathrm{\&alpha;}}_x\sin {\mathrm{\&alpha;}}_y& \cos {\mathrm{\&alpha;}}_x& -\cos {\mathrm{\&alpha;}}_y\sin {\mathrm{\&alpha;}}_y\\ -\sin {\mathrm{\&alpha;}}_y& 0& \cos {\mathrm{\&alpha;}}_y\end{array}\right],$

for

the error of calculation,

for the strapdown matrix calculating, for navigation is tied to the transition matrix that calculates navigation system,

for rotation is tied to the transition matrix that carrier is,

for zero-mean white Gaussian noise;

Under orientation large misalignment angle condition, velocity error equation is:

$\mathrm{\&delta;}{v}^{-n}=(I-C_{n^{\&prime;}}^n)C_b^{n^{\&prime;}}{f}^b-(2\mathrm{\&delta;}{w}_{\mathrm{ie}}^n+\mathrm{\&delta;}{w}_{\mathrm{en}}^n)\&times;({\hat{v}}^n-{\mathrm{\&delta;v}}^n)-(2{\hat{w}}_{\mathrm{ie}}^n+{\hat{w}}_{\mathrm{en}}^n)\&times;{\mathrm{\&delta;v}}^n+{\mathrm{\&delta;g}}^n+C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}(\mathrm{\&Delta;}{f}^b+w_a^b)$

Wherein, δ v ⁿ=[δ v _eδ v _x] ^tfor horizontal velocity error,

for δ v ⁿderivative,

for calculating navigation, be tied to the transition matrix of navigation system,

for carrier is tied to the transition matrix that navigation is, f ^δthe projection of fastening at carrier for accelerometer specific force,

for earth rotation angular speed is in the value of calculating navigation system, for the value of navigating and being is being calculated with respect to the angle of rotation speed of earth system by the system of navigating,

with

for

the error of calculation,

for the speed of navigation calculation,

for the measured deviation of accelerometer, ε g ⁿfor the error of calculation of gravitational vector,

zero-mean white Gaussian noise;

Under orientation large misalignment angle condition, the site error of carrier is not affected by it, and site error equation is:

$\left\{\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\frac{\mathrm{\&delta;}{v}_n}{R}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{\mathrm{\&delta;}{v}_e}{R}\sec L+\frac{v_n}{R}\tan L\sec \mathrm{L\&delta;L}\end{array}\right.$

Wherein,

be respectively the derivative of longitude, latitude error, δ L is longitude error, and L is longitude, and R is earth radius, and sec is secant, and tan is tan;

Step 3: the main object of carrier-borne rotary inertia guiding systems on-line proving is inertia device scale factor error and constant error item, according to the research to the qualitative question marked of inertia device parameter error, the whole Observables of the scale factor error of gyro and constant value drift, and accelerometer scale factor error and zero inclined to one side level are to all considerable, and sky is not to considerable, choose naval vessel site error, velocity error, attitude error, all gyroscope constant value drifts and gyro scale factor error, horizontal accelerometer zero partially and accelerometer scale factor error as filter status amount,

The system filter equation of supposing carrier-borne rotary inertial navigation is:

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

Wherein: x _kfor k quantity of state constantly, z _kfor k measurement amount constantly, f () and h () are respectively system nonlinear state function and measure function; u _kand v _kfor zero-mean white Gaussian noise;

The quantity of state that is located at strapdown inertial navitation system (SINS) filtering equations in line calibration technique is as follows:

x(t)=[δL?δλ?δv _e?δv _n?α _e?α _n?α _u?D _0x?D _0y?D _0z?A _0x?A _0y?ΔS _gx?ΔS _gy?ΔS _gx?ΔS _ax?ΔS _ay] ^T

Step 4: utilize the external units such as GPS, Doppler log and star sensor to provide high precision reference information for carrier-borne rotary strapdown inertial navitation system (SINS), position, speed, the attitude that itself and inertial navigation system are provided done relatively poor, and as the measurement amount of system filter device, amount is measured as:

z(t)=[L _INS-L?λ _INS-λ?v _eINS-v _e?v _nINS-v _N?α _eINS-α _e?α _nINS-α _n?α _uINS-α _u] ^T

Wherein, first position, speed and attitude information that is respectively inertial reference calculation of each element in measurement amount, second is respectively high precision position, speed and the attitude information that extraneous reference device provides;

Step 5: according to quantity of state dimension n and volume Kalman filtering algorithm characteristic, obtain cubature point position and weights and be respectively:

${\mathrm{\&xi;}}_i=\sqrt{n}{\left[1\right]}_i$

${\mathrm{\&omega;}}_i=\frac{1}{2n}$

Wherein, n is quantity of state dimension, i=1 ..., 2n, [1] _ithe i row that represent set [1], during for n=2, $\left[1\right]=\{\left[\begin{array}{c}1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\end{array}\right],\left[\begin{array}{c}-1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\end{array}\right]\};$

Step 6: according to the cubature point obtaining in step 5, by Cholesky, decompose, the cubature point that can obtain propagating in state equation, the position that system state equation is ordered due to cubature and the impact of weights thereof can be propagated as another new state equation relevant to cubature point, and then can obtain step status predication value and a status predication covariance matrix relevant to cubature point:

The position of ordering according to cubature and weights thereof, system state equation is propagated and is:

γ _i，k＋1|k＝f _k(ζ _i，k，u _k)+q _k，i=0，1，…，2n

Wherein, γ _{i, k+1|k}for the quantity of state in new state equation, q _kfor system noise,

In conjunction with volume Kalman filtering algorithm, can obtain a step status predication is:

${\hat{x}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{f}_k({\mathrm{\&xi;}}_{i,k},u_k)+q_k$

Status predication covariance matrix is:

$P_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T+Q_k$

Wherein, u _kn dimension control inputs vector, Q _kcovariance matrix for system noise;

Step 7: add one to become fading factor when multiple and make it have the uncertain robustness of answering system model in the status predication covariance matrix in step 6:

In order to make wave filter there is the uncertain robustness of answering system model, in status predication covariance matrix, introduce fading factor λ _k+1:

$P_{k+1|k}^{\left(l\right)}={\mathrm{\&lambda;}}_{k+1}(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T)+Q_k$

This fading factor is determined by following formula:

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_0<1\end{array}\right.$

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}{N}_{k+1}}{\mathrm{tr}{M}_{k+1}}$

$N_{k+1}=C_{0,k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}$

$M_{k+1}=H_{k+1}{\mathrm{\&Phi;}}_{k+1}{P}_{k+1|k}{\mathrm{\&Phi;}}_{k+1}^T{H}_{k+1}^T$

Wherein, λ _k+1>=1, trN _k+1and trM _k+1be respectively N _k+1and M _k+1mark, for H _k+1transposition, for Φ _k+1transposition, x _kfor k quantity of state constantly,

for k predicted state amount constantly, h _k+1for describing the arbitrary function of measurement equation; Q _kfor the covariance matrix of system noise, R _k+1for the covariance matrix of measurement noise, P _k+1|kfor status predication covariance matrix,

Meanwhile, C _{0, k}must meet:

$C_{0,k+1}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_1{{\mathrm{\&gamma;}}_1}^T& k=0\\ \frac{\mathrm{\&rho;}{C}_{0,k}+{\mathrm{\&gamma;}}_{k+1}{{\mathrm{\&gamma;}}_{k+1}}^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.$

Wherein, γ _k+1for the quantity of state constantly of k+1 in new state equation, 0 < ρ≤1 is forgetting factor, conventionally value is 0.95, forgetting factor can further improve the quick tracking power of wave filter, its value is larger, the shared ratio of the information of k before is constantly less, more can give prominence to the impact of current residual vector, wherein, the impact of residual vector is larger, tracking power is stronger, the introducing of fading factor and forgetting factor can make this filtering method have the extremely strong tracking power about mutation status, and when filtering reaches stable state, still can keep the tracking power to soft phase and mutation status, by above analysis, can realize the time of strong tracking volume Kalman filtering algorithm upgrades,

Step 8: according to the cuba ture point obtaining in step 5, by Cholesky, decompose, the cuba ture point that obtains propagating in measurement equation, the position that system measurements equation is ordered due to cuba ture and the impact of weights thereof can be propagated as another new measurement equation relevant with fading factor to cuba ture point:

Utilize Cholesky to decompose have:

$P_{k+1|k}^{\left(l\right)}=S_{k+1|k}{S}_{k+1|k}^T$

Wherein, S _k+1|kfor Spherical integration,

Cuba ture point that can be relevant to fading factor, for:

${\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}=S_{k+1|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k}$

The cuba ture point of propagating by measurement equation is:

$x_{i,k+1|k}^{\left(l\right)}=h_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

, system k+1 measurement predictor is constantly:

${\hat{z}}_{k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

Auto-correlation covariance matrix is:

$P_{\mathrm{zz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T+R_{k+1}$

Wherein, R _k+1for the covariance matrix of measurement noise,

Simple crosscorrelation covariance matrix is:

$P_{\mathrm{xz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

Step 9: derive volume Kalman filtering algorithm according to step 8 and measure auto-correlation covariance matrix and the simple crosscorrelation covariance matrix relevant with fading factor to cuba ture point in renewal, can obtain the kalman gain relevant with fading factor to cuba ture point, state estimation value and state error covariance estimated value, realize the renewal of the strong tracking volume Kalman filtering algorithm with strong tracking performance:

According to the algorithm of kalman gain, state estimation value and state error covariance estimated value in above filtering update algorithm and volume Kalman filtering algorithm, kalman gain that can be relevant to fading factor, state estimation value and state error covariance estimated value, be respectively:

$K_{k+1}^{\left(l\right)}=P_{\mathrm{xz},k+1|k}^{\left(l\right)}{\left(P_{\mathrm{zz},k+1|k}^{\left(l\right)}\right)}^{-1}$

${\hat{x}}_{k+1|k+1}^{\left(l\right)}={\hat{x}}_{k+1|k}^{\left(l\right)}+K_{k+1}^{\left(l\right)}(z_{k+1}^{\left(l\right)}-{\hat{z}}_{k+1|k}^{\left(l\right)})$

$P_{k+1|k+1}^{\left(l\right)}=P_{k+1|k}^{\left(l\right)}-K_{k+1|k}^{\left(l\right)}{P}_{\mathrm{zz},k+1|k}^{\left(l\right)}{\left(K_{k+1|k}^{\left(l\right)}\right)}^T;$

Step 10: with reference to the filtering process flow diagram of figure 2, according to the concrete time of strong tracking volume Kalman filtering algorithm in step 6 and step 8, upgrade and measure renewal process the quantity of state in step 3 is estimated, position, speed and the attitude error and the inertia device parameters error that obtain inertial navigation system, solved the Parameter Estimation Problem of carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving.

The present invention is under complicated marine environment, utilize and based on the carrier-borne rotary strapdown inertial navitation system (SINS) online calibration method of strong tracking volume kalman filter method, can realize the on-line proving of inertial navigation system in the present invention, compensate to a certain extent inertia device parameter error, improve inertial navigation system navigation accuracy, the method has kept the strong tracking power to mutation status, there is the stronger robustness about the change of real system parameter, estimated accuracy is high, without linearization, process calculate simple, computing time is short and can process exactly higher-dimension number system, be difficult for dispersing.

The foregoing is only preferred embodiment of the present invention, not in order to limit the present invention, all any modifications of doing within the spirit and principles in the present invention, be equal to and replace and improvement etc., within all should being included in protection scope of the present invention.

Claims (10)

1. a method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving, is characterized in that, the method for this carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving comprises the following steps:

Step 1, sets up the output error models of gyro, accelerometer and the error equation of inertial navigation system;

Step 2, chosen position, speed and attitude error and inertia device parameter error, as filter status amount, are set up state equation;

Step 3, the difference of the position that the position that the inertial navigation system of usining resolves, speed, attitude information and GPS, Doppler log, star sensor provide, speed, attitude information, as wave filter measurement amount, is set up measurement equation;

Step 4, according to the quantity of state dimension n of wave filter and volume Kalman filtering algorithm characteristic, determines the position ξ that cubature is ordered _iwith corresponding weights ω _i, i=1 wherein ..., 2n;

Step 5, according to the cubature point obtaining, by Cholesky, decompose, the cubature point that obtains propagating in state equation, the position that system state equation is ordered due to cubature and the impact of weights can be propagated as another new state equation relevant to cubature point, obtain step status predication value and a status predication covariance matrix relevant to cubature point;

Step 6, adds one to become fading factor when multiple in the status predication covariance matrix in step 5;

Step 7, according to the cubature point obtaining in step 4, by Cholesky, decompose, the cubature point that obtains propagating in measurement equation, the position that measurement equation is ordered due to cubature and the impact of weights thereof can be propagated as another new measurement equation relevant with fading factor to cubature point;

Step 8, according to step 7, derive auto-correlation covariance matrix, simple crosscorrelation covariance matrix, kalman gain, state estimation value and the state error covariance estimated value relevant with fading factor to cubature point, realize the measurement of the strong tracking volume Kalman filtering algorithm with strong tracing property and strong robustness and upgrade;

Step 9, according to the concrete time of strong tracking volume Kalman filtering algorithm in step 5 and step 7, upgrade and measure renewal process quantity of state in step 3 is estimated, obtain inertia device parameters error amount, realized carrier-borne strapdown inertial navitation system (SINS) on-line proving.

2. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 1, is characterized in that, in step 4, and the position ξ that cubature is ordered _iweights ω corresponding to it _i, i=1 ..., 2n is defined as:

${\mathrm{\&xi;}}_i=\sqrt{n}{\left[1\right]}_i$

${\mathrm{\&omega;}}_i=\frac{1}{2n}$

Wherein, [1] _ithe i row that represent set [1], during for n=2, have $\left[1\right]=\{\left[\begin{array}{c}1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\end{array}\right],\left[\begin{array}{c}-1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\end{array}\right]\},i=1,...,2n.$

3. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 1, is characterized in that, in step 6, adds one to become fading factor when multiple, in k+1 status predication covariance matrix constantly, add multiple time become fading factor λ _k+1be defined as:

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_0<1\end{array}\right.$

${\mathrm{\&lambda;}}_0=\frac{{\mathrm{trN}}_{k+1}}{{\mathrm{trM}}_{k+1}}$

$N_{k+1}=C_{0,k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}$

$M_{k+1}=H_{k+1}{\mathrm{\&Phi;}}_{k+1}{P}_{k+1|k}{\mathrm{\&Phi;}}_{k+1}^T{H}_{k+1}^T$

In formula, λ _k+1>=1, trN _k+1and trM _k+1be respectively N _k+1and M _k+1mark, the transposition of superscript T representing matrix,

x _kfor k quantity of state constantly,

for k predicted state amount constantly, h _k+1for describing the arbitrary function of measurement equation; Q _kfor the covariance matrix of system noise, R _k+1for the covariance matrix of measurement noise, P _k+1|kfor status predication covariance matrix, $C_{0,k+1}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_1{{\mathrm{\&gamma;}}_1}^T& k=0\\ \frac{\mathrm{\&rho;}{C}_{0,k}+{\mathrm{\&gamma;}}_{k+1}{{\mathrm{\&gamma;}}_{k+1}}^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.,$ γ _k+1for the quantity of state constantly of k+1 in new state equation, 0 < ρ≤1 is forgetting factor, and status predication covariance matrix adds becomes fading factor λ when multiple _k+1after become:

$P_{k+1|k}^{\left(l\right)}={\mathrm{\&lambda;}}_{k+1}(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T)+Q_k$

Wherein,

be a step status predication (utilizing k constantly to predict that k+1 constantly), ∑ is the symbol of suing for peace, and superscript (1) represents that parameter is subject to the impact of fading factor.

4. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 1, is characterized in that, in step 8, the auto-correlation covariance matrix obtaining is:

$P_{\mathrm{zz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T+R_{k+1}$

Wherein, for the quantity of state in new state equation,

for the position that cubature is ordered, i=1 ..., 2n,

Simple crosscorrelation covariance matrix is:

$P_{\mathrm{xz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

The kalman gain relevant to fading factor, state estimation value and state error covariance estimated value, be respectively:

$K_{k+1}^{\left(l\right)}=P_{\mathrm{xz},k+1|k}^{\left(l\right)}{\left(P_{\mathrm{zz},k+1|k}^{\left(l\right)}\right)}^{-1}$

${\hat{x}}_{k+1|k+1}^{\left(l\right)}={\hat{x}}_{k+1|k}^{\left(l\right)}+K_{k+1}^{\left(l\right)}(z_{k+1}^{\left(l\right)}-{\hat{z}}_{k+1|k}^{\left(l\right)})$

$P_{k+1|k+1}^{\left(l\right)}=P_{k+1|k}^{\left(l\right)}-K_{k+1|k}^{\left(l\right)}{P}_{\mathrm{zz},k+1|k}^{\left(l\right)}{\left(K_{k+1|k}^{\left(l\right)}\right)}^T$

Wherein,

represent contrary.

5. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 1, is characterized in that, the concrete steps of the method for this carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving are:

Step 1 needs the measured value of Real-time Collection gyro and accelerometer in the process of ship navigation, and the measured value of gyro is

the measured value of accelerometer is

superscript s representative rotation system; Subscript x, y, z represents respectively the x of gyro and acceleration, y, tri-axles of z, preserve the measured value of GPS, Doppler log and star sensor in real time;

Step 2: under orientation large misalignment angle condition, inertial navigation system error equation exists significantly non-linear, sets up the inertial navigation system nonlinearity erron equation relevant to inertia device parameter error;

Step 3: the main object of carrier-borne rotary inertia guiding systems on-line proving is inertia device scale factor error and constant error item, according to the qualitative question marked to inertia device parameter error, choose naval vessel site error, velocity error, attitude error, all gyroscope constant value drifts and gyro scale factor error, horizontal accelerometer zero partially and accelerometer scale factor error as filter status amount;

Step 5: according to quantity of state dimension n and volume Kalman filtering algorithm characteristic, obtain cubature point position and weights and be respectively:

${\mathrm{\&xi;}}_i=\sqrt{n}{\left[1\right]}_i$

${\mathrm{\&omega;}}_i=\frac{1}{2n}$

Wherein, n is quantity of state dimension, i=1 ..., 2n, [1] _ithe i row that represent set [1], during for n=2, $\left[1\right]=\{\left[\begin{array}{c}1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\end{array}\right],\left[\begin{array}{c}-1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\end{array}\right]\};$

Step 6: according to the cubature point obtaining in step 5, by Cholesky, decompose, the cubature point that obtains propagating in state equation, the position that system state equation is ordered due to cubature and the impact of weights can be propagated as another new state equation relevant to cubature point, obtain step status predication value and a status predication covariance matrix relevant to cubature point;

Step 7: add one to become fading factor when multiple and make it have the uncertain robustness of answering system model in the status predication covariance matrix in step 6:

In order to make wave filter there is the uncertain robustness of answering system model, in status predication covariance matrix, introduce fading factor λ _k+1:

$P_{k+1|k}^{\left(l\right)}={\mathrm{\&lambda;}}_{k+1}(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T)+Q_k$

This fading factor is determined by following formula:

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_0<1\end{array}\right.$

${\mathrm{\&lambda;}}_0=\frac{{\mathrm{trN}}_{k+1}}{{\mathrm{trM}}_{k+1}}$

$N_{k+1}=C_{0,k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}$

$M_{k+1}=H_{k+1}{\mathrm{\&Phi;}}_{k+1}{P}_{k+1|k}{\mathrm{\&Phi;}}_{k+1}^T{H}_{k+1}^T$

Wherein, λ _k+1>=1, trN _k+1and trM _k+1be respectively N _k+1and M _k+1mark,

for H _k+1transposition,

for Φ _k+1transposition,

x _kfor k quantity of state constantly,

for k predicted state amount constantly, h _k+1for describing the arbitrary function of measurement equation; Q _kfor the covariance matrix of system noise, R _k+1for the covariance matrix of measurement noise, P _k+1|kfor status predication covariance matrix,

Meanwhile, C _{0, k}must meet:

$C_{0,k+1}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_1{{\mathrm{\&gamma;}}_1}^T& k=0\\ \frac{\mathrm{\&rho;}{C}_{0,k}+{\mathrm{\&gamma;}}_{k+1}{{\mathrm{\&gamma;}}_{k+1}}^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.,$

Wherein, γ _k+1for the quantity of state constantly of k+1 in new state equation, 0 < ρ≤1 is forgetting factor, and value is 0.95 conventionally;

Step 8: according to the cubature point obtaining in step 5, by Cholesky, decompose, the cubature point that obtains propagating in measurement equation, the position that system measurements equation is ordered due to cubature and the impact of weights thereof can be propagated as another new measurement equation relevant with fading factor to cubature point;

Step 9: derive volume Kalman filtering algorithm according to step 8 and measure auto-correlation covariance matrix and the simple crosscorrelation covariance matrix relevant with fading factor to cubature point in renewal, can obtain the kalman gain relevant with fading factor to cubature point, state estimation value and state error covariance estimated value, realize the renewal of the strong tracking volume Kalman filtering algorithm with strong tracking performance:

According to the algorithm of kalman gain, state estimation value and state error covariance estimated value in above filtering update algorithm and volume Kalman filtering algorithm, kalman gain that can be relevant to fading factor, state estimation value and state error covariance estimated value;

Step 10: upgrade and measure renewal process according to the concrete time of strong tracking volume Kalman filtering algorithm in step 6 and step 8 the quantity of state in step 3 is estimated, obtain position, speed and attitude error and the inertia device parameters error of inertial navigation system.

6. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 5, is characterized in that, in step 2, and the angular velocity error ε of gyro output ^sspecific force error delta f with accelerometer output ^sbe respectively:

${\mathrm{\&epsiv;}}^s=D_0+{\mathrm{\&Delta;S}}_g{\hat{w}}^s$

${\mathrm{\&Delta;f}}^s=A_0+{\mathrm{\&Delta;S}}_a{\hat{f}}^s$

Wherein, Δ S _g=diag[Δ S _gxΔ S _gyΔ S _gz] be the scale factor error of gyro, D ₀=[D _0xd _0yd _0z] ^tfor the constant value drift of gyro, Δ S _a=diag[Δ S _axΔ S _ayΔ S _az] be accelerometer scale factor error, A ₀=[A _0xa _0ya _0z] ^tfor accelerometer bias, subscript g represents gyro, and a represents accelerometer;

Under orientation large misalignment angle condition, attitude error equations is:

$\stackrel{\&CenterDot;}{a}=C_w^{-1}[(I-C_n^{n^{\&prime;}}){\hat{w}}_{\mathrm{in}}^n+C_n^{n^{\&prime;}}{\mathrm{\&delta;w}}_{\mathrm{in}}^n-C_b^{n^{\&prime;}}({\mathrm{\&epsiv;}}^b+w_g^b)]$

Wherein, α=[α _eα _nα _u] ^tfor attitude error angle, subscript e, n, u represent respectively sky, northeast, and α is

derivative, subscript n representative navigation system, i represents inertial system, n ' representative is calculated navigation and is,

for the angle of rotation speed of navigation system with respect to inertial system, sin, cos are just being respectively, cosine function, $C_w^{-1}=\frac{1}{\cos {\mathrm{\&alpha;}}_x}\left[\begin{array}{ccc}\cos {\mathrm{\&alpha;}}_x\cos {\mathrm{\&alpha;}}_y& 0& \cos {\mathrm{\&alpha;}}_x\sin {\mathrm{\&alpha;}}_y\\ \sin {\mathrm{\&alpha;}}_x\sin {\mathrm{\&alpha;}}_y& \cos {\mathrm{\&alpha;}}_x& -\cos {\mathrm{\&alpha;}}_y\sin {\mathrm{\&alpha;}}_x\\ -\sin {\mathrm{\&alpha;}}_y& 0& \cos {\mathrm{\&alpha;}}_y\end{array}\right],$

for

the error of calculation,

for the strapdown matrix calculating,

for navigation is tied to the transition matrix that calculates navigation system,

for rotation is tied to the transition matrix that carrier is, for zero-mean white Gaussian noise;

Under orientation large misalignment angle condition, velocity error equation is:

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}^n=(I-C_{n^{\&prime;}}^n){C_b^{n^{\&prime;}}f}^b-(2\mathrm{\&delta;}{w}_{\mathrm{ie}}^n+\mathrm{\&delta;}{w}_{\mathrm{en}}^n)\&times;({\hat{v}}^n-\mathrm{\&delta;}{v}^n)-(2{\hat{w}}_{\mathrm{ie}}^n+{\hat{w}}_{\mathrm{en}}^n)\&times;\mathrm{\&delta;}{v}^n+{\mathrm{\&delta;v}}^n\\ +C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}(\mathrm{\&Delta;}{f}^b+w_a^b)\end{array}$

Wherein, δ v ⁿ=[δ v _eδ v _n] ^tfor horizontal velocity error,

for δ v ⁿderivative,

for calculating navigation, be tied to the transition matrix of navigation system,

for carrier is tied to the transition matrix that navigation is, f ^bthe projection of fastening at carrier for accelerometer specific force, for earth rotation angular speed is in the value of calculating navigation system,

for the value of navigating and being is being calculated with respect to the angle of rotation speed of earth system by the system of navigating,

know

for

the error of calculation,

for the speed of navigation calculation,

for the measured deviation of accelerometer, δ g ⁿfor the error of calculation of gravitational vector,

zero-mean white Gaussian noise;

Under orientation large misalignment angle condition, the site error of carrier is unaffected, and site error equation is:

$\left\{\begin{array}{c}\mathrm{\&delta;}\stackrel{.}{L}=\frac{\mathrm{\&delta;}{v}_n}{R}\\ \mathrm{\&delta;}\stackrel{.}{\mathrm{\&lambda;}}=\frac{\mathrm{\&delta;}{v}_e}{R}\sec L+\frac{v_n}{R}\tan L\sec \mathrm{L\&delta;L}\end{array}\right.$

Wherein,

be respectively the derivative of longitude, latitude error, δ L is longitude error, and L is longitude, and R is earth radius, and sec is secant, and tan is tan.

7. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 5, is characterized in that, in step 4, supposes that the system filter equation of carrier-borne rotary inertial navigation is:

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

Wherein: x _kfor k quantity of state constantly, z _kfor k measurement amount constantly, f () and h () are respectively system nonlinear state function and measure function; u _kand v _kfor zero-mean white Gaussian noise;

The quantity of state that is located at strapdown inertial navitation system (SINS) filtering equations in line calibration technique is as follows:

x(t)＝[δL?δλ?δv _e?δv _n?α _e?α _n?α _u?D _0x?D _0y?D _0z?A _0x?A _0y?ΔS _gx?ΔS _gy?ΔS _gz?ΔS _ax?ΔS _ay] ^T，

Utilize GPS, Doppler log and star sensor external unit to provide high precision reference information for carrier-borne rotary strapdown inertial navitation system (SINS), do poor comparison with position, speed, attitude that inertial navigation system provides, and as the measurement amount of system filter device, amount is measured as:

z(t)＝[L _INS-L?λ _INS-λ?v _eINS-v _e?v _nINS-v _N?α _eINS-α _e?α _nINS-α _n?α _uINS-α _u] ^T

Wherein, first position, speed and attitude information that is respectively inertial reference calculation of each element in measurement amount, second is respectively high precision position, speed and the attitude information that extraneous reference device provides.

8. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 5, is characterized in that, in step 6, and the position of ordering according to cubature and weights, system state equation is propagated and is:

γ _i,k+1|k＝f _k(ξ _i,k，u _k)+q _k，i＝0，1，…，2n

Wherein, γ _{i, k+1|k}for the quantity of state in new state equation, q _kfor system noise,

In conjunction with volume Kalman filtering algorithm, can obtain a step status predication is:

${\hat{x}}_{k+1|k}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{f}_k({\mathrm{\&xi;}}_{i,k},u_k)+q_k$

Status predication covariance matrix is:

$P_{k+1|k}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{i,k+1|k}{\mathrm{\&gamma;}}_{i,k+1|k}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T+Q_k$

Wherein, u _kn dimension control inputs vector, Q _kcovariance matrix for system noise.

9. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 5, is characterized in that, in step 8, new measurement equation is:

Utilize Cholesky to decompose

have:

$P_{k+1|k}^{\left(l\right)}=S_{k+1|k}{S}_{k+1|k}^T$

Wherein, S _k+1|kfor Spherical integration,

Cubature point that can be relevant to fading factor, for:

${\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}=S_{k+1|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k}$

The cubature point of propagating by measurement equation is:

$x_{i,k+1|k}^{\left(l\right)}=h_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

, system k+1 measurement predictor is constantly:

${\hat{z}}_{k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)$

Auto-correlation covariance matrix is:

$P_{\mathrm{zz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left(\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T+R_{k+1}$

Wherein, R _k+1for the covariance matrix of measurement noise,

Simple crosscorrelation covariance matrix is:

$P_{\mathrm{xz},k+1|k}^{\left(l\right)}=\frac{1}{2n}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}{\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)}^T-\frac{1}{2n}{\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\underset{i-1}{\overset{2n}{\mathrm{\&Sigma;}}}{h}_{k+1}\left({\mathrm{\&xi;}}_{i,k+1|k}^{\left(l\right)}\right).$

10. the method for carrier-borne rotary strapdown inertial navitation system (SINS) on-line proving as claimed in claim 5, is characterized in that, in step 9, the kalman gain that fading factor is relevant, state estimation value and state error covariance estimated value, be respectively:

$K_{k+1}^{\left(l\right)}=P_{\mathrm{xz},k+1|k}^{\left(l\right)}{\left(P_{\mathrm{zz},k+1|k}^{\left(l\right)}\right)}^{-1}$

${\hat{x}}_{k+1|k+1}^{\left(l\right)}={\hat{x}}_{k+1|k}^{\left(l\right)}+K_{k+1}^{\left(l\right)}(z_{k+1}^{\left(l\right)}-{\hat{z}}_{k+1|k}^{\left(l\right)})$

$P_{k+1|k+1}^{\left(l\right)}=P_{k+1|k}^{\left(l\right)}-K_{k+1|k}^{\left(l\right)}{P}_{\mathrm{zz},k+1|k}^{\left(l\right)}{\left(K_{k+1|k}^{\left(l\right)}\right)}^T.$

Images