CN101706287A - Rotating strapdown system on-site proving method based on digital high-passing filtering - Google Patents

Abstract

The invention provides a rotating strapdown system on-site proving method based on digital high-passing filtering, which comprises the steps: (1) determining an initial position parameter of a carrier by a GPS; (2) collecting data output by an optical fiber gyroscope and an acceleration meter, and processing the data; (3) rotating and stopping at four positions of a single axis of an inertia measurement unit; (4) analyzing biased observability degree of an inertia device by using a spectral condition number method; (5) filtering the Schuler period contained in velocity information of a navigation system by adopting an IIR high-passing digital filter; and (6) taking filtered velocity information as observed quantity, and estimating the deviation of the inertia device by adopting a Kalman filtering technique. When the carrier is in anchoring state, the adoption of the method can obtain higher on-site proving precision.

Description

A kind of rotating strapdown system field calibration method based on Digital High Pass Filter

Technical field

What the present invention relates to is a kind of measuring method, in particular a kind of rotating strapdown system field calibration method based on Digital High Pass Filter.

Background technology

Strapdown inertial navigation system (SINS) is connected in inertance elements such as gyroscope, accelerometer on the carrier, according to Newton mechanics law, by the information of these inertance element collections is handled, obtain the complete independent navigation equipment of the full navigation information such as attitude, speed, position, acceleration, angular velocity and angular acceleration of carrier.Because SINS relies on its own inertial element fully, do not rely on any external information to measure navigational parameter, therefore, it has good concealment, be not subjected to the weather condition restriction, advantage such as interference-free is a kind of complete autonomous type, round-the-clock navigational system, has been widely used in fields such as Aeronautics and Astronautics, navigation.According to the ultimate principle of SINS, the existence that SINS inertia device in navigation procedure often is worth deviation is the principal element that causes the inertial navigation system navigation accuracy to be difficult to improve.How effectively limiting the inertial navigation error, to disperse, improve the inertial navigation system precision be the very important problem in inertial navigation field.

The error of inertial navigation system suppresses, is not to depend on outside assisting error state is estimated, but the propagation law of research inertial navigation error under the special exercise condition, and limit error according to this rule and disperse, improve the method for navigation accuracy.Rotating inhibition is most typical error inhibition method: by around an axle or a plurality of rotator inertia measuring units (IMU), navigation error is modulated, reach the purpose that navigation accuracy is dispersed, improved to the control navigation error.

Calibration technique is exactly a kind of method that improves the actual service precision of inertial navigation from the software aspect.Calibration technique also is a kind of Error Compensation Technology in essence.So-called Error Compensation Technology is exactly to set up the error mathematic model of inertance element and inertial navigation system, determines model coefficient by certain test, and then eliminates error by software algorithm.Inertance element and inertial navigation system must be by demarcating to determine basic error mathematic model parameter, to guarantee the operate as normal of element and system before dispatching from the factory.And the error compensation under the abominable dynamic environment of research, inertial navigation system of inertance element high-order error term all is to carry out on the basis of demarcating, and we can say that staking-out work is the basis of whole Error Compensation Technology.The Inertial Measurement Unit error (drift of inertance element and scale factor error) of starting shooting one by one is different, and along with the time increases the IMU output error and drifts about in time, realizes that on-the-spot on-line proving is significant for improving system accuracy.

Open report related to the present invention in the CNKI storehouse has: 1, " horizontal initial alignment error is to the influence of rotation IMU accuracy of navigation systems ", this article has mainly been told about the influence of horizontal misalignment to the rotation strapdown inertial navitation system (SINS), tell about the unidirectional rotation of IMU single shaft in its Chinese, but do not mentioned the content of on-site proving.2, " application of rotation IMU in fiber strapdown boat appearance system ", this article has mainly been introduced single shaft, twin shaft rotation mode, and proves in theory.3, " the six positions rotation on-site proving new method of optical fibre gyro IMU ", this article has mainly been introduced at the scene of using optical fibre gyro IMU has been carried out the rotation of ten secondaries on six position, set up 42 non-linear input and output equations according to the error model of optical fibre gyro IMU then, disappear mutually with the symmetric position error by the rotation integration, eliminate the nonlinear terms in the equation, finally solve gyro constant multiplier, gyroscope constant value drift, gyro misalignment and accelerometer and often be worth 15 error coefficients such as biasing.

Summary of the invention

The object of the present invention is to provide a kind of carrier of working as to be under the moored condition, can obtain a kind of rotating strapdown system field calibration method of higher on-site proving precision based on Digital High Pass Filter.

The object of the present invention is achieved like this: may further comprise the steps:

(1) utilizes global position system GPS to determine the initial position parameters of carrier, they are bound to navigational computer;

(2) fiber optic gyro strapdown inertial navigation system carries out gathering after the preheating data of fibre optic gyroscope and quartz accelerometer output;

(3) IMU adopts 8 commentaries on classics to stop the transposition scheme that order is a swing circle;

(4) utilize the spectral condition method of counting to ask for the observability degree of inertia device deviation in the IMU four-position rotation and stop process;

(5) designing the infinite impulse response digital high-pass filter, is that the carrier horizontal velocity that calculates is down carried out the high-pass filtering processing with navigating, and the Schuler period under filtering is navigated and is in the bearer rate keeps carrier owing to the velocity deviation of waving and swinging the generation of moving;

Model is floated in estimating when (6) setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system, directly as observed quantity, utilizes Kalman Filter Technology to realize the on-site proving of rotation strapdown inertial navitation system (SINS) with the speed that obtains after the high-pass filtering.

The present invention can also comprise:

1, to adopt 8 commentaries on classics to stop order be that the transposition scheme of a swing circle is for described IMU:

Order 1, IMU clockwise rotates 180 ° of in-position C, stand-by time T from the A point _tOrder 2, IMU clockwise rotates 90 ° of in-position D, stand-by time T from the C point _tOrder 3, IMU rotates counterclockwise 180 ° of in-position B, stand-by time T from the D point _tOrder 4, IMU rotates counterclockwise 90 ° of in-position A, stand-by time T from the B point _tOrder 5, IMU rotates counterclockwise 180 ° of in-position C, stand-by time T from the A point _tOrder 6, IMU rotates counterclockwise 90 ° of in-position B, stand-by time T from the C point _tOrder 7, IMU clockwise rotates 180 ° of in-position D, stand-by time T from the B point _tOrder 8, IMU clockwise rotates 90 ° of in-position A, stand-by time T from the D point _tIMU rotates sequential loop according to this to carry out; IMU pause point p3, p8 and p4, p7 on the horizontal east orientation axle are symmetrical in the rotating shaft center; Pause point p1 on the north orientation axle, p5 and p2, p6 are symmetrical in the rotating shaft center; It is that carry out at 180 ° or 90 ° of intervals that four-position rotation and stop scheme remains rotational angle.

2, the described method of utilizing the spectral condition method of counting to ask for the observability degree of inertia device deviation in the IMU four-position rotation and stop process is:

Find the solution system of linear equations

AX＝b，b∈C ⁿ

If A ∈ is C ^{N * n}, || || be a kind of operator norm,

$\mathrm{cond}\left(A\right)=\left\{\begin{array}{cc}\left|\right|A\left|\right|\left|\right|A^{-1}\left|\right|,& \det A\≠0\\ \∞,& \det A=0\end{array}\right.$

Claim cond (A) be matrix A about operator norm || || conditional number, commonly used is about the p-norm || || _pConditional number, note is made cond _p(A), cond ₂(A) be the spectral condition number,

At discrete time-varying system:

$\left\{\begin{array}{c}{X}_{k+1}=F_k{X}_k\\ Z_k=H{X}_k\end{array}\right.$

Bring system state equation into observation equation and obtain a set of equations:

$\left\{\begin{array}{c}{Z}_0=H{X}_0\\ Z_1=H{F}_0{\mathrm{<figure-callout\; id="0"\; label="X"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">X</figure-callout>}}_0\\ .\\ .\\ .\\ Z_k=H\underset{i=0}{\overset{k}{\mathrm{\&Pi;}}}{F}_i{\mathrm{<figure-callout\; id="0"\; label="X"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">X</figure-callout>}}_0\end{array}\right.$

Note

$O_k={\left[\begin{array}{cc}\mathrm{<figure-callout\; id="0"\; label="H\; HF"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">H</figure-callout>}& {\mathrm{HF}}_{}{}_0...H\underset{i=0}{\overset{k}{\mathrm{\&Pi;}}}{F}_i\end{array}\right]}^T$

Then

O _kX ₀＝Z

For stational system F _kBe constant, O _kBe exactly the ornamental matrix, time-varying system is observed on sampled point and is obtained discrete time-varying system, F _kBe exactly the state-transition matrix Φ (t in the sampling period _k+ T, t _k),

O _k＝[HHΦ(t ₁，t ₀)…HΦ(t _k，t ₀)] ^T

State is the n dimension, an observed quantity Z _kBe r dimension (r＜n), the order of observation battle array H is r, carries out k time at least and observes (kr 〉=n), obtain X ₀, find the solution state X according to least square method ₀,

${\mathrm{<figure-callout\; id="0"\; label="X"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">X</figure-callout>}}_0={\left(O_k^T{O}_k\right)}^{-1}{O}_k^{T}Z$

Be the observation battle array, because Be normal matrix, count cond by calculating spectral condition ₂(M), analyze stability of solution, and

${\mathrm{cond}}_2\left(M\right)=\frac{\underset{\mathrm{\&lambda;}\&Element;\mathrm{\&lambda;}\left(M\right)}{\max }\left|\mathrm{\&lambda;}\right|}{\underset{\mathrm{\&lambda;}\&Element;\mathrm{\&lambda;}\left(M\right)}{\min }\left|\mathrm{\&lambda;}\right|}$

In the formula, λ is the eigenwert of matrix M, eigenwert and the proper vector of further analysis matrix M, so that determine that the observability degree of which state is better actually, the observability degree of which state is poor, but with M diagonalization at the tenth of the twelve Earthly Branches, note U ^TMU=Λ, wherein Λ=diag (λ ₁, λ ₂... λ _n), the observability degree S of state X then:

S＝abs(U[λ ₁，λ ₂，...，λ _n] ^T)

Calculate eigenwert and the proper vector of the observability matrix M of system, determine the observability degree of each state.

3, the described method of utilizing Kalman Filter Technology to realize the on-site proving of rotation strapdown inertial navitation system (SINS) is:

1) set up the state equation of Kalman filtering:

The state error of rotation strapdown inertial navitation system (SINS) is described with linear first-order differential equation:

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)$

The state vector of etching system when wherein X (t) is t; A (t) and B (t) are respectively the state matrix and the noise matrix of system; W (t) is the system noise vector;

The state vector of system is:

The white noise vector of system is:

W＝[a _x?a _y?ω _x?ω _y?ω _z?0?0?0?0?0?0?0?0] ^T

δ V wherein _e, δ V _nThe velocity error of representing east orientation, north orientation respectively; Be respectively IMU coordinate system ox _s, oy _sAxis accelerometer zero partially; ε _x, ε _y, ε _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant value drift of axle gyro; a _x, a _yBe respectively IMU coordinate system ox _s, oy _sThe white noise error of axis accelerometer; δ K _Gx, δ K _Gy, δ K _GzBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant multiplier error of axle gyro; ω _x, ω _y, ω _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe white noise error of axle gyro;

The state-transition matrix of system is:

$A=\left[\begin{array}{cccc}{\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;2}^1& {\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">f</figure-callout>}}_{2\&times;3}& {\tilde{T}}_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="O"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">O</figure-callout>}}_{2\&times;6}\\ {\mathrm{<figure-callout\; id="3"\; label="F"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">F</figure-callout>}}_{3\&times;2}^2& {\mathrm{<figure-callout\; id="3"\; label="F"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">F</figure-callout>}}_{3\&times;3}^3& {\mathrm{<figure-callout\; id="3"\; label="O"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">O</figure-callout>}}_{3\&times;2}& {\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">T</figure-callout>}}_{3\&times;6}\\ {\mathrm{<figure-callout\; id="8"\; label="O"\; filenames="H2009100732422E0000021.png,H2009100732422E0000022.png"\; state="\{\{state\}\}">O</figure-callout>}}_{8\&times;2}& {\mathrm{<figure-callout\; id="8"\; label="O"\; filenames="H2009100732422E0000021.png,H2009100732422E0000022.png"\; state="\{\{state\}\}">O</figure-callout>}}_{8\&times;3}& {\mathrm{<figure-callout\; id="8"\; label="O"\; filenames="H2009100732422E0000021.png,H2009100732422E0000022.png"\; state="\{\{state\}\}">O</figure-callout>}}_{8\&times;2}& {\mathrm{<figure-callout\; id="8"\; label="O"\; filenames="H2009100732422E0000021.png,H2009100732422E0000022.png"\; state="\{\{state\}\}">O</figure-callout>}}_{8\&times;6}\end{array}\right]$

${\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;2}^1=\left[\begin{array}{cc}\frac{V_N}{R_n}\tan \mathrm{<figure-callout\; id="2"\; label="L"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">L</figure-callout>}& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{{2V}_E}{R_n}\tan L)& 0\end{array}\right]$

${\mathrm{<figure-callout\; id="3"\; label="F"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">F</figure-callout>}}_{3\&times;2}^2=\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{{\mathrm{<figure-callout\; id="0"\; label="R\; n"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">R</figure-callout>}}_n}& 0\\ \frac{\tan \mathrm{<figure-callout\; id="0"\; label="L\; R\; n"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">L</figure-callout>}}{R_n}& 0\end{array}\right]$

${\mathrm{<figure-callout\; id="3"\; label="F"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">F</figure-callout>}}_{3\&times;3}^3=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_{\mathrm{<figure-callout\; id="0"\; label="N\; R\; m"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">N</figure-callout>}}}{R_m}& 0\end{array}\right]$

${\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">f</figure-callout>}}_{2\&times;3}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right]$

${\tilde{T}}_{2\&times;2}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]$

${\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">T</figure-callout>}}_{3\&times;6}=\left[\begin{array}{cccccc}-T_{11}& -T_{12}& -T_{13}& -T_{11}{\mathrm{\&omega;}}_x& -{\mathrm{<figure-callout\; id="12"\; label="T"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">T</figure-callout>}}_{12}{\mathrm{\&omega;}}_y& -T_{13}{\mathrm{\&omega;}}_z\\ -T_{21}& -T_{22}& -T_{23}& -T_{21}{\mathrm{\&omega;}}_x& -T_{22}{\mathrm{\&omega;}}_y& -T_{23}{\mathrm{\&omega;}}_z\\ -T_{31}& -T_{32}& -T_{33}& -T_{31}{\mathrm{\&omega;}}_x& -T_{32}{\mathrm{\&omega;}}_y& -T_{33}{\mathrm{\&omega;}}_z\end{array}\right]$

V _E, V _NThe speed of representing east orientation, north orientation respectively; ω _x, ω _y, ω _zThree input angular velocities representing gyro respectively; ω _IeThe expression rotational-angular velocity of the earth; R _m, R _nRepresent earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches respectively; L represents local latitude; f _E, f _N, f _UBe expressed as respectively navigation coordinate system down east orientation, north orientation, day to specific force;

2) set up the measurement equation of Kalman filtering:

The measurement equation of describing the rotation strapdown inertial navitation system (SINS) with linear first-order differential equation is as follows:

Z(t)＝H(t)X(t)+V(t)

Wherein: the measurement vector of etching system during Z (t) expression t; The measurement matrix of H (t) expression system; The measurement noise of V (t) expression system;

The system measurements matrix is:

$H=\left[\begin{array}{ccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

Amount is measured as the speed that obtains after the high-pass filtering:

$Z={\left[\begin{array}{cc}{\tilde{V}}_E& {\tilde{V}}_N\end{array}\right]}^T.$

Four positions that the present invention is fixing with the relative carrier azimuth axis of Inertial Measurement Unit are changeed and are stopped, utilize the moving observability degree that improves systematic parameter of commentaries on classics stoppage in transit of IMU, the design Kalman filter is introduced each calibrated error item that the high precision oracle encourages IMU respectively, estimate and compensation IMU output error, finish the on-site proving of system.

The present invention's advantage compared with prior art is: the present invention has broken the on-site proving that traditional scaling method is not suitable for system, proposing a kind of IMU of utilization commentaries on classics stops improving the systematic parameter observability degree and adopts Kalman Filter Technology the inertia device deviation to be carried out the scheme of on-line proving, this method inertia device often can be worth deviation and gyroscope constant multiplier error is carried out scene estimation and compensation, can improve system alignment and navigation accuracy effectively.

The effect useful to the present invention is described as follows:

Under Visual C++ simulated conditions, this method is carried out emulation experiment:

Setting the gyroscope constant value drift is 0.01 °/h, and the accelerometer zero drift is 10 ^-4G; System's initial alignment error is 0.1 °, 0.1 °, 0.5 °; Carrier is done the three-axis swinging motion with sinusoidal rule around pitch axis, axis of roll and course axle, and its mathematical model is:

$\left\{\begin{array}{c}\mathrm{\&theta;}={\mathrm{\&theta;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&theta;}}t+{\mathrm{\&phi;}}_{\mathrm{\&theta;}})\\ \mathrm{\&gamma;}={\mathrm{\&gamma;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&gamma;}}t+{\mathrm{\&phi;}}_{\mathrm{\&gamma;}})\\ \mathrm{\&psi;}={\mathrm{\&psi;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&psi;}}t+{\mathrm{\&phi;}}_{\mathrm{\&psi;}})+k\end{array}\right.$

Wherein: θ, γ, ψ represent the angle variables of waving of pitch angle, roll angle and course angle respectively; θ _m, γ _m, ψ _mThe angle amplitude is waved in expression accordingly respectively; ω _θ, ω _γ, ω _ψRepresent corresponding angle of oscillation frequency respectively; φ _θ, φ _γ, φ _ψRepresent corresponding initial phase respectively; ω _i=2 τ/T _i, i=θ, γ, ψ, T _iRepresent corresponding rolling period, k is the angle, initial heading.Get during emulation: θ _m=6 °, γ _m=12 °, ψ _m=10 °, T _θ=8s, T _γ=10s, T _ψ=6s, k=0 °.

The swaying of carrier, surging and hang down and swing the acceleration that causes and be:

In the formula, i=x, y, z be geographic coordinate system east orientation, north orientation, day to.

For going up, [0,2 π] obey equally distributed random phase.

Carrier initial position: 45.7796 ° of north latitude, 126.6705 ° of east longitudes;

The initial attitude error angle: three initial attitude error angles are zero;

Equatorial radius: R _e=6378393.0m;

Ellipsoid degree: e=3.367e-3;

The earth surface acceleration of gravity that can get by universal gravitation: g ₀=9.78049;

Rotational-angular velocity of the earth (radian per second): 7.2921158e-5;

The gyroscope constant value drift: 0.01 degree/hour;

The gyroscope random walk: 0.001 degree/

Gyroscope constant multiplier error: 1000ppm;

Accelerometer bias: 10 ^-4g ₀

Accelerometer noise: 10 ^-6g ₀

Constant: π=3.1415926;

The mathematical model parameter of IMU four-position rotation and stop scheme:

The dead time of four positions: T _t=5min;

The time that consumes when rotating 180 ° and 90 °: T _z=12s;

Rotate in the process of 180 ° and 90 °, the acceleration and deceleration time in each transposition respectively is 4s.

Description of drawings

Fig. 1 is a kind of rotating strapdown system field calibration method process flow diagram based on Digital High Pass Filter of the present invention;

Fig. 2 is in the IMU four-position rotation and stop process of the present invention, the relative position relation of IMU coordinate system and carrier coordinate system;

Fig. 3 is the gyroscope constant value drift of estimation of the present invention;

Fig. 4 is the accelerometer bias on the horizontal direction of estimation of the present invention;

Fig. 5 is the gyroscope constant multiplier error of estimation of the present invention.

Embodiment

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

(1) utilizes global position system GPS to determine the initial position parameters of carrier, they are bound to navigational computer;

(2) fiber optic gyro strapdown inertial navigation system carries out gathering after the preheating data of fibre optic gyroscope and quartz accelerometer output;

(3) IMU adopts 8 commentaries on classics to stop the transposition scheme that order is a swing circle;

Order 1, IMU clockwise rotates 180 ° of in-position C, stand-by time T from the A point _tOrder 2, IMU clockwise rotates 90 ° of in-position D, stand-by time T from the C point _tOrder 3, IMU rotates counterclockwise 180 ° of in-position B, stand-by time T from the D point _tOrder 4, IMU rotates counterclockwise 90 ° of in-position A, stand-by time T from the B point _tOrder 5, IMU rotates counterclockwise 180 ° of in-position C, stand-by time T from the A point _tOrder 6, IMU rotates counterclockwise 90 ° of in-position B, stand-by time T from the C point _tOrder 7, IMU clockwise rotates 180 ° of in-position D, stand-by time T from the B point _tOrder 8, IMU clockwise rotates 90 ° of in-position A, stand-by time T from the D point _tIMU rotates sequential loop according to this to carry out.Positive and negative average in order effectively the inertia device deviation on the horizontal direction to be carried out on symmetric position, IMU pause point p3, p8 and p4, p7 on the horizontal east orientation axle of definition are symmetrical in the rotating shaft center; Pause point p1 on the north orientation axle, p5 and p2, p6 are symmetrical in the rotating shaft center.It is that carry out at 180 ° or 90 ° of intervals that improved four-position rotation and stop scheme remains rotational angle.

(4) utilize the spectral condition method of counting to ask for the observability degree of inertia device deviation in the IMU four-position rotation and stop process;

Consider to find the solution system of linear equations

AX＝b，b∈C ⁿ (1)

If A ∈ is C ^{N * n}, || || be a kind of operator norm,

$\mathrm{cond}\left(A\right)=\left\{\begin{array}{cc}\left|\right|A\left|\right|\left|\right|A^{-1}\left|\right|,& \det A\&NotEqual;0\\ \&infin;,& \det A=0\end{array}\right.---\left(2\right)$

Claim cond (A) be matrix A (about inverting or finding the solution system of linear equations) about operator norm || || conditional number.Commonly used is about the p-norm || || _pConditional number, can remember and make cond _p(A).Especially, claim cond ₂(A) be the spectral condition number.

At discrete time-varying system:

$\left\{\begin{array}{c}{X}_{k+1}=F_k{X}_k\\ Z_k=H{X}_k\end{array}\right.---\left(3\right)$

Can be by a group observations Z=[Z ₀, Z ₁... .Z _k] ^TObtain original state X ₀Be the considerable essence of system.Bring system state equation into observation equation and obtain a set of equations:

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="0"\; label="Z"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">Z</figure-callout>}}_0=H{X}_0\\ Z_1=H{F}_0{\mathrm{<figure-callout\; id="0"\; label="X"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">X</figure-callout>}}_0\\ .\\ .\\ .\\ Z_k=H\underset{i=0}{\overset{k}{\mathrm{\&Pi;}}}{F}_i{X}_0\end{array}\right.---\left(4\right)$

Note

$O_k={\left[\begin{array}{cc}\mathrm{<figure-callout\; id="0"\; label="H\; HF"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">H</figure-callout>}& {\mathrm{HF}}_{}{}_0...H\underset{i=0}{\overset{k}{\mathrm{\&Pi;}}}{F}_i\end{array}\right]}^T---\left(5\right)$

Then have

O _kX ₀＝Z (6)

For stational system F _kBe constant, O _kIt is exactly the ornamental matrix.Time-varying system is observed on sampled point and is obtained discrete time-varying system, F _kBe exactly the state-transition matrix Φ (t in the sampling period _k+ T, t _k), therefore have

O _k＝[H?HΦ(t ₁，t ₀)…HΦ(t _k，t ₀)] ^T (7)

If state is the n dimension, an observed quantity Z _kBe r dimension (r＜n), the order of observation battle array H is r, carries out k time at least and observes (kr 〉=n), just can obtain X ₀Find the solution state X according to least square method ₀

${\mathrm{<figure-callout\; id="0"\; label="X"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">X</figure-callout>}}_0={\left(O_k^T{O}_k\right)}^{-1}{O}_k^{T}Z---\left(8\right)$

Note

Be the observation battle array.Because

Be normal matrix, then can count cond by calculating spectral condition ₂(M), analyze stability of solution, and

${\mathrm{cond}}_2\left(M\right)=\frac{\underset{\mathrm{\&lambda;}\&Element;\mathrm{\&lambda;}\left(M\right)}{\max }\left|\mathrm{\&lambda;}\right|}{\underset{\mathrm{\&lambda;}\&Element;\mathrm{\&lambda;}\left(M\right)}{\min }\left|\mathrm{\&lambda;}\right|}---\left(9\right)$

In the formula, λ is the eigenwert of matrix M.Eigenwert and the proper vector of further analysis matrix M, so that determine that the observability degree of which state is better actually, the observability degree of which state is poor.But with M diagonalization at the tenth of the twelve Earthly Branches, note U ^TMU=Λ, wherein Λ=diag (λ ₁, λ ₂... λ _n), the observability degree S of state X then:

S＝abs(U[λ ₁，λ ₂，...，λ _n] ^T) (10)

Calculate eigenwert and the proper vector of the observability matrix M of system, just can determine the observability degree of each state.

(5) design infinite impulse response digital high-pass filter (IIR), the carrier horizontal velocity that navigation system calculates is down carried out the high-pass filtering processing, Schuler period under filtering is navigated and is in the bearer rate keeps carrier owing to wave and swing the velocity deviation of the generation of moving;

Model is floated in estimating when (6) setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system, with the speed that obtains after the high-pass filtering directly as observed quantity.Utilize Kalman Filter Technology to realize the on-site proving of rotation strapdown inertial navitation system (SINS);

Foundation is the Kalman filter model of observed quantity with the horizontal velocity under being through the navigation after the high-pass filtering;

1) set up the state equation of Kalman filtering:

The state error of rotation strapdown inertial navitation system (SINS) is described with linear first-order differential equation:

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(11\right)$

The state vector of etching system when wherein X (t) is t; A (t) and B (t) are respectively the state matrix and the noise matrix of system; W (t) is the system noise vector;

The state vector of system is:

The white noise vector of system is:

W＝[a _x?a _y?ω _x?ω _y?ω _z?0?0?0?0?0?0?0?0] ^T (13)

δ V wherein _e, δ V _nThe velocity error of representing east orientation, north orientation respectively;

Be respectively IMU coordinate system ox _s, oy _sAxis accelerometer zero partially; ε _x, ε _y, ε _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant value drift of axle gyro; a _x, a _yBe respectively IMU coordinate system ox _s, oy _sThe white noise error of axis accelerometer; δ K _Gx, δ K _Gy, δ K _GzBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant multiplier error of axle gyro; ω _x, ω _y, ω _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe white noise error of axle gyro;

The state-transition matrix of system is:

$A=\left[\begin{array}{cccc}{\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;2}^1& {\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">f</figure-callout>}}_{2\&times;3}& {\tilde{T}}_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="O"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">O</figure-callout>}}_{2\&times;6}\\ {\mathrm{<figure-callout\; id="3"\; label="F"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">F</figure-callout>}}_{3\&times;2}^2& {\mathrm{<figure-callout\; id="3"\; label="F"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">F</figure-callout>}}_{3\&times;3}^3& {\mathrm{<figure-callout\; id="3"\; label="O"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">O</figure-callout>}}_{3\&times;2}& {\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">T</figure-callout>}}_{3\&times;6}\\ {\mathrm{<figure-callout\; id="8"\; label="O"\; filenames="H2009100732422E0000021.png,H2009100732422E0000022.png"\; state="\{\{state\}\}">O</figure-callout>}}_{8\&times;2}& {\mathrm{<figure-callout\; id="8"\; label="O"\; filenames="H2009100732422E0000021.png,H2009100732422E0000022.png"\; state="\{\{state\}\}">O</figure-callout>}}_{8\&times;3}& {\mathrm{<figure-callout\; id="8"\; label="O"\; filenames="H2009100732422E0000021.png,H2009100732422E0000022.png"\; state="\{\{state\}\}">O</figure-callout>}}_{8\&times;2}& {\mathrm{<figure-callout\; id="8"\; label="O"\; filenames="H2009100732422E0000021.png,H2009100732422E0000022.png"\; state="\{\{state\}\}">O</figure-callout>}}_{8\&times;6}\end{array}\right]---\left(14\right)$

${\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;2}^1=\left[\begin{array}{cc}\frac{V_N}{R_n}\mathrm{<figure-callout\; id="2"\; label="tan\; L"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">tan</figure-callout>}L& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{{2V}_E}{R_n}\tan L)& 0\end{array}\right]---\left(15\right)$

${\mathrm{<figure-callout\; id="3"\; label="F"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">F</figure-callout>}}_{3\&times;2}^2=\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{{\mathrm{<figure-callout\; id="0"\; label="R\; n"\; filenames="H2009100732422E0000022.png,H2009100732422E0000032.png"\; state="\{\{state\}\}">R</figure-callout>}}_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]---\left(16\right)$

${\mathrm{<figure-callout\; id="3"\; label="F"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">F</figure-callout>}}_{3\&times;3}^3=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_N}{R_m}& 0\end{array}\right]---\left(17\right)$

${\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">f</figure-callout>}}_{2\&times;3}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right]---\left(18\right)$

${\tilde{T}}_{2\&times;2}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]---\left(19\right)$

${\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="H2009100732422E0000032.png"\; state="\{\{state\}\}">T</figure-callout>}}_{3\&times;6}=\left[\begin{array}{cccccc}-T_{11}& -T_{12}& -T_{13}& -T_{11}{\mathrm{\&omega;}}_x& -{\mathrm{<figure-callout\; id="12"\; label="T"\; filenames="H2009100732422E0000022.png,H2009100732422E0000031.png"\; state="\{\{state\}\}">T</figure-callout>}}_{12}{\mathrm{\&omega;}}_y& -T_{13}{\mathrm{\&omega;}}_z\\ -T_{21}& -T_{22}& -T_{23}& -T_{21}{\mathrm{\&omega;}}_x& -T_{22}{\mathrm{\&omega;}}_y& -T_{23}{\mathrm{\&omega;}}_z\\ -T_{31}& -T_{32}& -T_{33}& -T_{31}{\mathrm{\&omega;}}_x& -T_{32}{\mathrm{\&omega;}}_y& -T_{33}{\mathrm{\&omega;}}_z\end{array}\right]---\left(20\right)$

V _E, V _NThe speed of representing east orientation, north orientation respectively; ω _x, ω _y, ω _zThree input angular velocities representing gyro respectively; ω _IeThe expression rotational-angular velocity of the earth; R _m, R _nRepresent earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches respectively; L represents local latitude; f _E, f _N, f _UBe expressed as respectively navigation coordinate system down east orientation, north orientation, day to specific force.

2) set up the measurement equation of Kalman filtering:

The measurement equation of describing the rotation strapdown inertial navitation system (SINS) with linear first-order differential equation is as follows:

Z(t)＝H(t)X(t)+V(t) (21)

Wherein: the measurement vector of etching system during Z (t) expression t; The measurement matrix of H (t) expression system; The measurement noise of V (t) expression system;

The system measurements matrix is:

$H=\left[\begin{array}{ccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(22\right)$

Amount is measured as the speed that obtains after the high-pass filtering:

$Z={\left[\begin{array}{cc}{\tilde{V}}_E& {\tilde{V}}_N\end{array}\right]}^T---\left(23\right)$

Claims (4)

1. rotating strapdown system field calibration method based on Digital High Pass Filter is characterized in that may further comprise the steps:

(1) utilizes global position system GPS to determine the initial position parameters of carrier, they are bound to navigational computer;

(2) fiber optic gyro strapdown inertial navigation system carries out gathering after the preheating data of fibre optic gyroscope and quartz accelerometer output;

(3) IMU adopts 8 commentaries on classics to stop the transposition scheme that order is a swing circle;

(4) utilize the spectral condition method of counting to ask for the observability degree of inertia device deviation in the IMU four-position rotation and stop process;

(5) designing the infinite impulse response digital high-pass filter, is that the carrier horizontal velocity that calculates is down carried out the high-pass filtering processing with navigating, and the Schuler period under filtering is navigated and is in the bearer rate keeps carrier owing to the velocity deviation of waving and swinging the generation of moving;

Model is floated in estimating when (6) setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system, directly as observed quantity, utilizes Kalman Filter Technology to realize the on-site proving of rotation strapdown inertial navitation system (SINS) with the speed that obtains after the high-pass filtering.

2. a kind of rotating strapdown system field calibration method based on Digital High Pass Filter according to claim 1 is characterized in that it is that the transposition scheme of a swing circle is that described IMU adopts 8 commentaries on classics to stop order:

Order 1, IMU clockwise rotates 180 ° of in-position C, stand-by time T from the A point _tOrder 2, IMU clockwise rotates 90 ° of in-position D, stand-by time T from the C point _tOrder 3, IMU rotates counterclockwise 180 ° of in-position B, stand-by time T from the D point _tOrder 4, IMU rotates counterclockwise 90 ° of in-position A, stand-by time T from the B point _tOrder 5, IMU rotates counterclockwise 180 ° of in-position C, stand-by time T from the A point _tOrder 6, IMU rotates counterclockwise 90 ° of in-position B, stand-by time T from the C point _tOrder 7, IMU clockwise rotates 180 ° of in-position D, stand-by time T from the B point _tOrder 8, IMU clockwise rotates 90 ° of in-position A, stand-by time T from the D point _tIMU rotates sequential loop according to this to carry out; IMU pause point p3, p8 and p4, p7 on the horizontal east orientation axle are symmetrical in the rotating shaft center; Pause point p1 on the north orientation axle, p5 and p2, p6 are symmetrical in the rotating shaft center; It is that carry out at 180 ° or 90 ° of intervals that four-position rotation and stop scheme remains rotational angle.

3. a kind of rotating strapdown system field calibration method based on Digital High Pass Filter according to claim 2 is characterized in that the described method of utilizing the spectral condition method of counting to ask for the observability degree of inertia device deviation in the IMU four-position rotation and stop process is:

Find the solution system of linear equations

AX＝b，b∈C ⁿ

If A ∈ is C ^{N * n}, || || be a kind of operator norm,

$\mathrm{cond}\left(A\right)=\left\{\begin{array}{cc}\left|\right|A\left|\right|\left|\right|A^{-1}\left|\right|,& \det A\&NotEqual;0\\ \&infin;,& \det A=0\end{array}\right.$

Claim cond (A) be matrix A about operator norm || || conditional number, commonly used is about the p-norm || || _pConditional number, note is made cond _p(A), cond ₂(A) be the spectral condition number,

At discrete time-varying system:

$\left\{\begin{array}{c}{X}_{k+1}=F_k{X}_k\\ Z_k={\mathrm{HX}}_k\end{array}\right.$

Bring system state equation into observation equation and obtain a set of equations:

$\left\{\begin{array}{c}{Z}_0={\mathrm{HX}}_0\\ Z_1={\mathrm{HF}}_0{X}_0\\ .\\ .\\ .\\ Z_k=H\underset{i=0}{\overset{k}{\mathrm{\&Pi;}}}{F}_i{X}_0\end{array}\right.$

Note

$O_k={\left[\begin{array}{cc}H& {\mathrm{HF}}_0...H\underset{i=0}{\overset{k}{\mathrm{\&Pi;}}}{F}_i\end{array}\right]}^T$

Then

O _kX ₀＝Z

For stational system F _kBe constant, O _kBe exactly the ornamental matrix, time-varying system is observed on sampled point and is obtained discrete time-varying system, F _kBe exactly the state-transition matrix Φ (t in the sampling period _k+ T, t _k),

O _k＝[H?HΦ(t ₁，t ₀)…HΦ(t _k，t ₀)] ^T

State is the n dimension, an observed quantity Z _kBe r dimension (r＜n), the order of observation battle array H is r, carries out k time at least and observes (kr 〉=n), obtain X ₀, find the solution state X according to least square method ₀,

$X_0={\left(O_k^T{O}_k\right)}^{-1}{O}_k^{T}Z$

Be the observation battle array, because

Be normal matrix, count cond by calculating spectral condition ₂(M), analyze stability of solution, and

${\mathrm{cond}}_2\left(M\right)=\frac{\underset{\mathrm{\&lambda;}\&Element;\mathrm{\&lambda;}\left(M\right)}{\max }\left|\mathrm{\&lambda;}\right|}{\underset{\mathrm{\&lambda;}\&Element;\mathrm{\&lambda;}\left(M\right)}{\min }\left|\mathrm{\&lambda;}\right|}$

In the formula, λ is the eigenwert of matrix M, eigenwert and the proper vector of further analysis matrix M, so that determine that the observability degree of which state is better actually, the observability degree of which state is poor, but with M diagonalization at the tenth of the twelve Earthly Branches, note U ^TMU=Λ, wherein Λ=diag (λ ₁, λ ₂... λ _n), the observability degree S of state X then:

S＝abs(U[λ ₁，λ ₂，...，λ _n] ^T)

Calculate eigenwert and the proper vector of the observability matrix M of system, determine the observability degree of each state.

4. a kind of rotating strapdown system field calibration method based on Digital High Pass Filter according to claim 3 is characterized in that the described method of utilizing Kalman Filter Technology to realize the on-site proving of rotation strapdown inertial navitation system (SINS) is:

1) set up the state equation of Kalman filtering:

The state error of rotation strapdown inertial navitation system (SINS) is described with linear first-order differential equation:

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)$

The state vector of etching system when wherein X (t) is t; A (t) and B (t) are respectively the state matrix and the noise matrix of system; W (t) is the system noise vector;

The state vector of system is:

The white noise vector of system is:

W＝[a _x?a _y?ω _x?ω _y?ω _z?0?0?0?0?0?0?0?0] ^T

δ V wherein _e, δ V _nThe velocity error of representing east orientation, north orientation respectively;

Be respectively IMU coordinate system ox _s, oy _sAxis accelerometer zero partially; ε _x, ε _y, ε _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant value drift of axle gyro; a _x, a _yBe respectively IMU coordinate system ox _s, oy _sThe white noise error of axis accelerometer; δ K _Gx, δ K _Gy, δ K _GzBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant multiplier error of axle gyro; ω _x, ω _y, ω _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe white noise error of axle gyro;

The state-transition matrix of system is:

$A=\left[\begin{array}{cccc}{F}_{2\&times;2}^1& f_{2\&times;3}& {\tilde{T}}_{2\&times;2}& O_{2\&times;6}\\ F_{3\&times;2}^2& F_{3\&times;3}^3& O_{3\&times;2}& T_{3\&times;6}\\ O_{8\&times;2}& O_{8\&times;3}& O_{8\&times;2}& O_{8\&times;6}\end{array}\right]$

$F_{2\&times;2}^1=\left[\begin{array}{cc}\frac{V_N}{R_n}\tan L& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{{2V}_E}{R_n}\tan L)& 0\end{array}\right]$

$F_{3\&times;2}^2=\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]$

$F_{3\&times;3}^3=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_N}{R_m}& 0\end{array}\right]$

$f_{2\&times;3}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right]$

${\tilde{T}}_{2\&times;2}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]$

$T_{3\&times;6}=\left[\begin{array}{cccccc}{-T}_{11}& -T_{12}& {-T}_{13}& -T_{11}{\mathrm{\&omega;}}_x& -T_{12}{\mathrm{\&omega;}}_y& {-T}_{13}{\mathrm{\&omega;}}_z\\ -T_{21}& -T_{22}& -T_{23}& -T_{21}{\mathrm{\&omega;}}_x& -T_{22}{\mathrm{\&omega;}}_y& -T_{23}{\mathrm{\&omega;}}_z\\ {-T}_{31}& -T_{32}& -T_{33}& -T_{31}{\mathrm{\&omega;}}_x& -T_{32}{\mathrm{\&omega;}}_y& -T_{33}{\mathrm{\&omega;}}_z\end{array}\right]$

V _E, V _NThe speed of representing east orientation, north orientation respectively; ω _x, ω _y, ω _zThree input angular velocities representing gyro respectively; ω _IeThe expression rotational-angular velocity of the earth; R _m, R _nRepresent earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches respectively; L represents local latitude; f _E, f _N, f _UBe expressed as respectively navigation coordinate system down east orientation, north orientation, day to specific force;

2) set up the measurement equation of Kalman filtering:

The measurement equation of describing the rotation strapdown inertial navitation system (SINS) with linear first-order differential equation is as follows:

Z(t)＝H(t)X(t)+V(t)

Wherein: the measurement vector of etching system during Z (t) expression t; The measurement matrix of H (t) expression system; The measurement noise of V (t) expression system;

The system measurements matrix is:

$H=\left[\begin{array}{ccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

Amount is measured as the speed that obtains after the high-pass filtering:

$Z={\left[\begin{array}{cc}{\tilde{V}}_E& {\tilde{V}}_N\end{array}\right]}^T.$

Images