CN103791918A - Polar region moving base alignment method for naval vessel strapdown inertial navigation system - Google Patents

Abstract

The invention discloses a polar region moving base alignment method for a naval vessel strapdown inertial navigation system. The polar region moving base alignment method comprises the following steps of selecting a fine alignment model of the SINS (strapdown inertial navigation system) in an inertial system, and constructing a misalignment angle equation and a speed error equation of a polar region moving base of the SINS; and constructing a mathematical model for kalman filtering, namely a state equation and a measurement equation, for polar region moving base alignment under the inertial system by taking an error value as a state value and a speed as an observation value, estimating the state value, and compensating a strapdown matrix through an estimated misalignment angle to realize polar region moving base alignment. The alignment mechanism of the polar region moving base alignment method is different from a conventional alignment method, so that adverse effects on a compass effect are eliminated; alignment on the polar region moving base of the naval vessel strapdown inertial navigation system can be realized; the polar region moving base alignment method has an important significance for realizing the polar region navigation for naval vessels.

Description

A kind of Strapdown Inertial Navigation System for Ship polar region moving alignment method

Technical field

The invention belongs to inertial navigation field, what relate to is the initial alignment on moving base method of a kind of strapdown inertial navitation system (SINS) (Strapdown Inertial Navigation System, SINS).Specifically utilize the Kalman filtering aligning under inertial system to realize Strapdown Inertial Navigation System for Ship polar region moving alignment.

Background technology

Strapdown inertial navigation system (SINS) is a kind of autonomous type, round-the-clock navigational system completely, has now been widely used in the military field such as naval vessel, aircraft.The ultimate principle of inertial navigation is according to the carrier acceleration recording, and tries to achieve speed and position through integral operation.Carry out integral operation and first will determine starting condition, this just need to carry out initial alignment to inertial navigation system.So-called initial alignment is exactly before inertial navigation system not yet formally enters navigation duty, sets up the necessary starting condition of navigational state, and for SINS, initial alignment is exactly to determine the initial value of strapdown matrix.Obviously, the precision of initial alignment will produce directly impact to the later normal working performance of system, be one of gordian technique of inertial navigation system.

For Strapdown Inertial Navigation System for Ship, the Kalman filtering that conventional Initial Alignment Method mainly contains under compass aligning and geographic coordinate system is at present aimed at.But due to the defect in mechanism, these two kinds of methods are only applicable to middle low latitudes, cannot realize polar region and aim at.First, the aligning mechanism that compass is aimed at is compass effect, but dies down with the rising compass effect of latitude, causes alignment precision to decline; And the biography letter from inertial device error to misalignment, along with the rising north orientation acceleration zero of latitude partially and east orientation gyroscopic drift will increase the impact of orientation misalignment, the rising that is latitude can make the system alignment time increase, and the orientation misalignment that east orientation gyroscopic drift produces is increased.Secondly, aim at for the Kalman filtering under Department of Geography, due to the existence of compass item, the evaluated error of the misalignment that the gyroscopic drift of unobservale quantity east orientation causes increases with the rising of latitude.And the impact that can be summed up as compass effect generation of above these undesirable elements.

Along with each military power more and more payes attention to the right of speech of polar region, the contention of the resource such as oil gas, territory to polar region is day by day fierce, and China naval vessel is also more and more frequent in the activity of polar region.Just must have independent polar navigation ability but China naval vessel will be increased in the essence existence of polar region, naturally just become one of the key content that will study as the initial alignment of its gordian technique polar region.Therefore the polar region of, realizing Strapdown Inertial Navigation System for Ship is aimed at having important strategic importance.

Summary of the invention

The object of the present invention is to provide a kind of scheme that realizes Strapdown Inertial Navigation System for Ship polar region initial alignment on moving base, utilize the Kalman filtering aligning under inertial system to solve polar region alignment issues.

The object of the present invention is achieved like this:

Step 1: set up the error model of polar region moving alignment under inertial system according to the ultimate principle of SINS, model comprises misalignment equation and velocity error equation;

Step 2: the error model that utilizes above step 1 to obtain, partially as quantity of state, set up state equation take velocity error, misalignment, gyroscope constant value drift and acceleration zero; The difference of the speed under the inertial system that the speed of resolving using SINS under inertial system and GPS record, as measurement amount, is set up measurement equation.State equation and measurement equation form the Kalman filter model of polar region moving alignment;

Step 3: the Kalman filter model that utilizes above step 2 to obtain, according to Kalman filtering fundamental equation, quantity of state is estimated;

Step 4: the accurate estimated value pair of utilizing the misalignment that above step 3 obtains

revise, obtain more accurate

Step 5: utilize above step 4 to obtain

in conjunction with

solve the initial value of strapdown matrix

realize polar region moving alignment.

Advantage of the present invention is mainly reflected in:

Select the Kalman filtering under inertial system to aim at as polar region moving alignment scheme, the harmful effect that can avoid the latitude being caused by compass effect to raise to initial alignment generation, because this alignment methods is to utilize the coupling of acceleration of gravity and orientation misalignment

realize, do not have compass effect.Moreover, resolve reference data comprehensive GPS supplementary using inertial system as SINS, can effectively isolate the move impact of the interference bringing of carrier angular velocity and line, realize SINS moving alignment.

Accompanying drawing explanation

Fig. 1 is the process flow diagram that under inertial system of the present invention, Kalman filtering is aimed at.

Fig. 2 is the simulation curve at latitude of the present invention roll error angle while being 89 °.

Fig. 3 is the simulation curve at latitude of the present invention pitching error angle while being 89 °.

Fig. 4 is the simulation curve at latitude of the present invention course error angle while being 89 °.

Embodiment

Below in conjunction with specific embodiment, the present invention is described in detail.

Step 1: set up the error model of polar region moving alignment under inertial system according to the ultimate principle of SINS, model comprises misalignment equation and velocity error equation

(1) misalignment equation

The misalignment here

refer to the error angle calculating between inertial system i ' and desirable inertial system i, error angle establishing equation process is as follows:

A) calculating inertial system i ', desirable inertial system i and carrier is the transition matrix between b the differential equation be respectively:

${\stackrel{\·}{C}}_b^{i^{\′}}=C_b^{i^{\′}}({\widetilde{\mathrm{\ω}}}_{\mathrm{ib}}^b\×)=C_b^{i^{\′}}({\mathrm{\ω}}_{\mathrm{ib}}^b\×+{\mathrm{\ϵ}}^b\×)---\left(1\right)$

${\stackrel{\·}{C}}_b^{i^{\′}}=C_b^{i^{\′}}({\mathrm{\ω}}_{\mathrm{ib}}^b\×)$

In formula

the antisymmetric matrix of the projection of fastening at carrier with respect to the measured value of the angular velocity of rotation of inertial system for carrier system;

the antisymmetric matrix of the projection of fastening at carrier with respect to the ideal value of the angular velocity of rotation of inertial system for carrier system; ε ^bthe antisymmetric matrix of × the projection of fastening at carrier for gyroscopic drift.

B) establishing error angle matrix is

right

both sides differentiate simultaneously is also considered for low-angle obtains:

In formula for the antisymmetric matrix of misalignment projection in inertial system.

Formula (1) substitution formula (2) is obtained

Further launch

C) in formula (4), ignore second order in a small amount

and can obtain misalignment angle equation by the similarity transformation theorem of matrix:

Above formula is further divided and is solved:

In formula ${\mathrm{\ϵ}}_b^b=\left[\begin{array}{ccc}{\mathrm{\ϵ}}_{\mathrm{bx}}^b& {\mathrm{\ϵ}}_{\mathrm{by}}^b& {\mathrm{\ϵ}}_{\mathrm{bz}}^b\end{array}\right]$ For gyroscope constant value drift is in the projection of carrier system, ${\mathrm{\ϵ}}_w^b=\left[\begin{array}{ccc}{\mathrm{\ϵ}}_{\mathrm{wx}}^b& {\mathrm{\ϵ}}_{\mathrm{wy}}^b& {\mathrm{\ϵ}}_{\mathrm{wz}}^b\end{array}\right]$ For Modelling of Random Drift of Gyroscopes is in the projection of carrier system, and be assumed to be Gauss white-noise process.

(2) velocity error equation

A) inertial navigation fundamental equation projected to respectively to inertial system i and calculate inertial system i ':

$\begin{array}{c}{f}^i={\stackrel{\·}{V}}^i+{{\mathrm{\ω}}^i}_{\mathrm{ie}}\×V^i-g^i\\ f^{i^{\′}}={\stackrel{\·}{V}}^{i^{\′}}+{{\mathrm{\ω}}^{i^{\′}}}_{\mathrm{ie}}\×V^{i^{\′}}-g^{i^{\′}}\end{array}---\left(6\right)$

F in formula ⁱfor the projection in inertial system of the ideal value of accelerometer output; f ^{i '}for the real output value of accelerometer is in the projection of calculating in inertial system; V ⁱfor the projection of desirable ground velocity under inertial system, recorded by GPS; V ^{i '}for Computed Ground Speed is in the projection of calculating under inertial system; ω ⁱ _ie, ω ^{i '} _iebe respectively earth rotation angular speed in inertial system and calculate the projection in inertial system; g ⁱ, g ^{i '}be respectively terrestrial gravitation acceleration in inertial system and calculate the projection in inertial system.

B) two formulas in (6) are made to difference and can obtain the velocity error equation that moves pedestal under inertial system:

$\mathrm{\δ}{\stackrel{\·}{V}}^i=C_b^{i^{\′}}{\tilde{f}}^b-C_b^i{f}^b-{{\mathrm{\ω}}^i}_{\mathrm{ie}}\×{\mathrm{\δV}}^i-{{\mathrm{\δ\ω}}^i}_{\mathrm{ie}}\×V^i+{\mathrm{\δg}}^i$

δ V in formula ⁱfor computing velocity and GPS record the difference of speed, i.e. δ V ⁱ=V ^{i '}-V ⁱ;

f ^bbe respectively real output value and the idea output of accelerometer; δ ω ⁱ _ie, δ g ⁱbe respectively the poor of its corresponding calculated value and ideal value, i.e. δ ω ⁱ _ie=ω ^{i '} _ie-ω ⁱ _ie, δ g ⁱ=g ^{i '}-g ⁱ.

C) to b) medium velocity error equation

item carries out abbreviation, and detailed process is as follows:

$C_b^{i^{\′}}{\tilde{f}}^b-C_b^i{f}^b=C_b^{i^{\′}}(f^b+{\▿}^b)-C_b^i{f}^b$

Due to when the actual computation ${{\mathrm{\ω}}^i}_{\mathrm{ie}}={\left[\begin{array}{ccc}0& 0& {\mathrm{\ω}}_{\mathrm{ie}}\end{array}\right]}^T$ Can be accurately known, so get here ${{\mathrm{\ω}}^{i^{\′}}}_{\mathrm{ie}}={{\mathrm{\ω}}^i}_{\mathrm{ie}}={\left[\begin{array}{ccc}0& 0& {\mathrm{\ω}}_{\mathrm{ie}}\end{array}\right]}^T$ Be δ ω ⁱ _ie=0, consider that the earth is that regular spheroid is δ g simultaneously ⁱ=0, velocity error equation abbreviation is:

In formula ${\▿}_b^b=\left[\begin{array}{ccc}{\▿}_{\mathrm{bx}}^b& {\▿}_{\mathrm{by}}^b& {\▿}_{\mathrm{bz}}^b\end{array}\right]$ For the accelerometer error of zero is in the projection of carrier system, ${\▿}_w^b=\left[\begin{array}{ccc}{\▿}_{\mathrm{wx}}^b& {\▿}_{\mathrm{wy}}^b& {\▿}_{\mathrm{wz}}^b\end{array}\right]$ For accelerometer random deviation is in the projection of carrier system, be assumed to be equally Gauss white-noise process.

Be the moving pedestal error model under the SINS inertial system that will set up of the present invention with above formula (5), (7).

Step 2: utilize the error equation under the inertial system that above step 1 obtains, take the margin of error as quantity of state, take speed as observed quantity, set up Kalman filter model under inertial system

(1) state equation

The error model of SINS under comprehensive inertial system, partially as quantity of state, can set up following state equation take velocity error, misalignment, gyroscope constant value drift and acceleration zero:

$\stackrel{\&CenterDot;}{X}=\left[\begin{array}{cccc}-({\mathrm{\&omega;}}_{\mathrm{ie}}^i\&times;)& f^i\&times;& 0_{3\&times;3}& C_b^i\\ 0_{3\&times;3}& 0_{3\&times;3}& -C_b^i& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]X+\left[\begin{array}{cccc}{C}_b^i& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& -C_b^i& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]\left[\begin{array}{c}{\&dtri;}_w^b\\ {\mathrm{\&epsiv;}}_w^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="HDA0000464589010000021.png,HDA0000464589010000022.png"\; state="\{\{state\}\}">b</figure-callout>}}\\ 0_{3\&times;1}\\ 0_{3\&times;1}\end{array}\right]---\left(8\right)$

In formula

for state variable, ε _x, ε _y, ε _zbe respectively the gyroscope constant value drift on carrier coordinate system x, y, z three axles, ▽ _x, ▽ _y, ▽ _zbe respectively the accelerometer zero drift on carrier coordinate system x, y, z axle;

for the antisymmetric matrix of earth rotation angular speed, expression is:

${\mathrm{\&omega;}}_{\mathrm{ie}}^i\&times;=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_{\mathrm{iez}}^i& {\mathrm{\&omega;}}_{\mathrm{iey}}^i\\ {\mathrm{\&omega;}}_{\mathrm{iez}}^i& 0& -{\mathrm{\&omega;}}_{\mathrm{iex}}^i\\ -{\mathrm{\&omega;}}_{\mathrm{iey}}^i& {\mathrm{\&omega;}}_{\mathrm{iex}}^i& 0\end{array}\right];$

F ⁱ× for the ideal value of accelerometer output is at the antisymmetric matrix of inertial system projection, expression is: $f^i={\stackrel{\&CenterDot;}{V}}^i+{{\mathrm{\&omega;}}^i}_{\mathrm{ie}}\&times;V^i-g^i.$

V in formula ⁱcan utilize GPS to record; Acceleration of gravity is at the projection g of inertial system ⁱcan try to achieve by following formula:

$g^i=C_n^i{g}^n=\left[\begin{array}{c}-g\cos L\cos [\mathrm{\&lambda;}+{\mathrm{\&omega;}}_{\mathrm{ie}}(t-t_0)]\\ -g\cos L\sin [\mathrm{\&lambda;}+{\mathrm{\&omega;}}_{\mathrm{ie}}(t-t_0)]\\ -g\sin L\end{array}\right]$

T in formula ₀represent to aim at the zero hour; λ, L be t moment carrier through, latitude.

(2) measurement equation

Get the difference of the speed under the inertial system that speed that under inertial system, SINS resolves and GPS record as observed quantity, the measurement equation that can obtain Kalman filtering under inertial system is:

$Z=\left[\begin{array}{c}{V}_x^{i^{\&prime;}}-V_x^i\\ V_y^{i^{\&prime;}}-V_y^i\\ V_z^{i^{\&prime;}}-V_z^i\end{array}\right]=\mathrm{HX}+V_w---\left(9\right)$

In formula

for the SINS computing speed on inertial system x, y, z axle, for the speed that on inertial system x, y, z axle, GPS records; V _wfor measurement noise sequence; H is for measuring battle array, and its expression is:

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

It is the mathematical model of the moving pedestal Kalman filtering of SINS under the inertial system that will set up of the present invention with above formula (8), (9).

Step 3: the Kalman filter model that utilizes above step 2 to obtain, according to Kalman filtering fundamental equation, quantity of state is estimated

(1) continuous Filtering Model is carried out to discretize processing, can obtain state equation and the measurement equation of Kalman Filtering for Discrete device:

$\left\{\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+{\mathrm{\&Gamma;}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

X in formula _kfor the estimated value of k moment quantity of state; Φ _{k, k-1}represent the Matrix of shifting of a step in k-1 moment to k moment; Γ _k-1the system noise that represents the k-1 moment drives battle array; W _k-1represent the system incentive noise sequence in k-1 moment; H _krepresent the measurement battle array in k moment; For the measurement noise sequence in k moment.

(2) establishing the measuring value in k moment is Z _k, according to Kalman filtering fundamental equation, state X _kestimation can solve by following process:

State one-step prediction: ${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}$

State estimation: ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})$

Filter gain: $K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}$

One-step prediction mean square deviation: $P_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+Q_{k-1}$

Estimate square error: $P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^{-1}+K_k{R}_k{K}_k^T$

K in formula _kfor the gain matrix in k moment; P _{k, k-1}represent the one-step prediction estimation variance battle array in k-1 moment to k moment; P _k-1represent the state estimation variance in k-1 moment; Q _kfor the variance battle array of system noise sequence, nonnegative definite; R _kfor the variance battle array of measurement noise sequence, positive definite.

Step 4: the accurate estimated value pair of utilizing the misalignment that above step 3 obtains

revise, obtain more accurate

The misalignment estimating by Kalman filter is

in low-angle situation, the error matrix that can obtain representing with misalignment:

In formula for the projection on inertial system x, y, z three axles of the estimated value of misalignment.

Right

revise, that is:

In formula

for essence to the finish time carrier be b to the transition matrix that calculates inertial system i ', upgrade and calculate by gyrostatic output.

Step 5: utilize above step 4 to obtain

in conjunction with

solve the initial value of strapdown matrix

realize polar region moving alignment

Above 4 steps have realized SINS have been registered to inertial system, and in reality, are to be using Department of Geography as navigation coordinate, therefore also need a step conversion could realize polar region and aim at, that is:

$C_b^n=C_i^n{C}_b^i$

In formula

for inertia is tied to the transition matrix that navigation is, can be determined by following formula:

$C_i^n\left(t_2\right)=\left[\begin{array}{ccc}-\sin [\mathrm{\&lambda;}+{\mathrm{\&omega;}}_{\mathrm{ie}}(t_2-t_0)]& \cos [\mathrm{\&lambda;}+{\mathrm{\&omega;}}_{\mathrm{ie}}(t_2-t_0)]& 0\\ -\sin L\cos [\mathrm{\&lambda;}+{\mathrm{\&omega;}}_{\mathrm{ie}}(t_2-t_0)]& -\sin L\sin [\mathrm{\&lambda;}+{\mathrm{\&omega;}}_{\mathrm{ie}}(t_2-t_0)]& \cos L\\ \cos L\cos [\mathrm{\&lambda;}+{\mathrm{\&omega;}}_{\mathrm{ie}}(t_2-t_0)]& \cos L\sin [\mathrm{\&lambda;}+{\mathrm{\&omega;}}_{\mathrm{ie}}(t_2-t_0)]& \sin L\end{array}\right]$

T in formula ₀, t ₂be respectively the zero hour and aim at the finish time; λ, L are respectively the current warp of carrier, latitude.

Beneficial effect of the present invention is verified by the following method:

By Matlab emulation, the output signal of gyro and accelerometer is produced by SINS simulator.Simulated conditions arranges as follows:

1) constant value drift of gyro is:

wherein (i=x, y, z);

The angle random walk coefficient of gyro is:

wherein (i=x, y, z);

The normal value of accelerometer is biased to: ▽ _bi=1 × 10 ^-4g, wherein (i=x, y, z);

Accelerometer measures white noise is:

wherein (i=x, y, z);

Naval vessel is under swinging condition, and its course angle ψ, pitch angle θ, roll angle γ are as follows:

Wherein

obey equally distributed random phase for [0,2 π] is upper.

2) the each component of the starting condition of Kalman filtering: X (0) all gets 0;

P(0)＝diag{0，0，0，(0.008743°) ²，(0.006721°) ²，(0.006721°) ²，

(0.01°/h) ²，(0.01°/h) ²，(0.01°/h) ²，(10 ^-4g) ²，(10 ^-4g) ²，(10 ^-4g) ²};

Q(0)＝diag{(10 ^-5g) ²，(10 ^-5g)2 ^，(10 ^-5g) ²，(0.001°/h) ²，(0.001°/h) ²，(0.001°/h) ²，0，0，0，0，0，0}

3) for moving pedestal emulation, the speed of establishing ship navigation is V _x=10m/s, V _y=10m/s.

4) be checking feasibility of the present invention, degree of learning from else's experience is 126.6705 °, and 89 °, latitude carries out the emulation of 500s fine alignment.

Simulation result: from Fig. 2 to Fig. 4, in the time that latitude is 89 °, fine alignment roll error finish time angle is-0.11 jiao point, pitching error angle is 0.2 jiao point, course error angle is-5.81 jiaos points, all meet the requirement of inertial navigation system to alignment precision, a kind of Strapdown Inertial Navigation System for Ship of visible the present invention polar region moving alignment method can realize polar region, naval vessel moving alignment.

The content not being described in detail in instructions of the present invention belongs to the known prior art of those skilled in the art.

Should be understood that, for those of ordinary skills, can be improved according to the above description or convert, and all these improvement and conversion all should belong to the protection domain of claims of the present invention.

Claims (3)

1. a Strapdown Inertial Navigation System for Ship polar region moving alignment method, is characterized in that, comprises the following steps:

Step 1: set up the error model of polar region moving alignment under inertial system according to the ultimate principle of SINS, model comprises misalignment equation and velocity error equation;

Step 2: the error model that utilizes step 1 to obtain, partially as quantity of state, set up state equation take velocity error, misalignment, gyroscope constant value drift and acceleration zero; The difference of the speed under the inertial system that the speed of resolving using SINS under inertial system and GPS record, as measurement amount, is set up measurement equation; State equation and measurement equation form the Kalman filter model of polar region moving alignment;

Step 3: the Kalman filter model that utilizes step 2 to obtain, according to Kalman filtering fundamental equation, quantity of state is estimated;

Step 4: the accurate estimated value pair of utilizing the misalignment that step 3 obtains

revise, obtain more accurate

Step 5: utilize step 4 to obtain

in conjunction with

solve the initial value of strapdown matrix realize polar region moving alignment.

2. method according to claim 1, is characterized in that, the foundation of step 1 medium velocity error equation: the difference of the speed under the inertial system that the speed of resolving using SINS under inertial system and GPS record is δ V as velocity error ⁱ=V ^{i '}-V ⁱ, and ignore earth rotation angular speed error delta ω ⁱ _iewith acceleration of gravity error delta g ⁱobtain following velocity error equation

F in formula ⁱfor the ideal value of accelerometer output is in the projection of inertial system;

ω ⁱ _iebe respectively the projection in inertial system of misalignment and earth rotation angular speed;

for carrier be b to the transition matrix that calculates inertial system i, upgrade and calculate by gyrostatic output; ▽ ^bfor accelerometer bias.

3. method according to claim 1, is characterized in that, in step 2, take velocity error, misalignment as quantity of state, and the gyroscopic drift of SINS and acceleration zero are carried out to modeling is partially extended for state variable; The mathematical model that the difference of the speed under the inertial system that the speed that SINS under inertial system is resolved and GPS record is set up Kalman filtering as observed quantity is as follows:

$\stackrel{\&CenterDot;}{X}=\left[\begin{array}{cccc}-({\mathrm{\&omega;}}_{\mathrm{ie}}^i\&times;)& f^i\&times;& 0_{3\&times;3}& C_b^i\\ 0_{3\&times;3}& 0_{3\&times;3}& -C_b^i& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]X+\left[\begin{array}{cccc}{C}_b^i& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& -C_b^i& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]\left[\begin{array}{c}{\&dtri;}_w^b\\ {\mathrm{\&epsiv;}}_w^b\\ 0_{3\&times;1}\\ 0_{3\&times;1}\end{array}\right]$

$Z=\left[\begin{array}{c}{V}_x^{i^{\&prime;}}-V_x^i\\ V_y^{i^{\&prime;}}-V_y^i\\ V_z^{i^{\&prime;}}-V_z^i\end{array}\right]=\left[\begin{array}{cccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]X+V_w$

In formula

for quantity of state; f ⁱ× be respectively

the antisymmetric matrix of fi;

for the lower gyroscope constant value drift of carrier system; for the lower accelerometer error of zero of carrier system;

for the computing speed of SINS on inertial system x, y, z axle, for the speed that on inertial system x, y, z axle, GPS records; V _wfor measurement noise sequence.

Images