CN1851406A - Gasture estimation and interfusion method based on strapdown inertial nevigation system - Google Patents

Abstract

The invention discloses a state estimating and interfusion method based on strap down inertial navigation system that includes the following steps: using the sensor in IMU to induce the kinetic characteristic; taking strap down inertial computing; using the state signal of acceleration estimating system; judging the reliability of state estimating value; taking state information interfusion; outputting navigation parameter. The invention has the following advantages: no additional hardware cost, fully autonomy; effectively improving the navigation accuracy of the system; and supplying 50 navigation signals per second to control display device.

Description

Based on the attitude estimation of strapdown inertial navigation system and the method that merges

One, technical field

The present invention relates to a kind of airmanship of strapdown inertial navigation system, relate in particular to the method that a kind of strapdown inertial navigation system utilizes the acceleration estimation attitude.

Two, background technology

Strapdown inertial navigation system (Strapdown Inertial Navigation System is called for short SINS) is the mechanics law according to the relative inertness space of newton's proposition, utilize inertance elements such as gyroscope, accelerometer to experience the acceleration of moving body, carry out the navigational parameters such as attitude, speed and position that integral operation obtains movable body by computing machine.In these navigational parameters, the attitude parameter error can directly influence the precision of other parameter, has even more important meaning so improve attitude measurement accuracy.

The guidance precision of SINS depends primarily on the precision of inertia device (gyroscope and accelerometer).The precision of INS is good in the time of starting working and lacking.But after initial alignment, because the accumulation of gyrostatic drift error, As time goes on precision can reduce.Improve the precision of strapdown INS at present, mainly take two technology to improve: (1) adopts more high-precision inertia device; (2) adopt the noncumulative external reference information of guidance missdistance that SINS is revised.

Above-mentioned two kinds of methods all can improve the navigation accuracy of SINS.But adopt more high-precision inertia device, the cost of the system that can be multiplied, this is that many applications are all unacceptable.If adopt the external information reference source, not only can increase system cost, reduce the independence of system, and can be subjected to the more reduction that causes system reliability of disturbing.In addition, in some complicated applied environments, can't use effective oracle.How under the situation that does not increase system cost, utilize inertia components and parts information to improve accuracy of navigation systems more fully as far as possible, a lot of application scenarios are had great importance.

Three, summary of the invention

1, goal of the invention: the purpose of this invention is to provide a kind of navigation control method that can effectively improve the strapdown inertial navigation system of system's navigational parameter precision.

2, technical scheme:, the present invention includes the following step in order to reach above-mentioned goal of the invention:

(1) utilize sensor sensing carrier movement characteristic in the six degree of freedom inertial measurement cluster (be called for short IMU): IMU by the responsive motion carrier of gyroscope along its axial angular velocity signal, by accelerometer measures along carrier shaft to the linear acceleration signal, and signal is transferred to navigational computer;

(2) carrying out strap down inertial navigation resolves: navigational computer carries out attitude algorithm to the angular velocity signal of gyroscope sensitivity, attitude matrix is carried out trigonometric function calculate attitude angle and the position angle that promptly extracts carrier, the linear acceleration of degree of will speed up instrumentation amount is converted into geographic coordinate system by carrier coordinate system, and then the back integration obtains navigational parameters such as speed, position;

(3) when carrying out step (2), carry out this step, utilize the attitude signal of acceleration estimation system: according to the relation between three axis accelerometer output, geographic coordinate system and the carrier coordinate system, when system's acceleration is very little, ignore the influence of speed and acceleration, obtain accelerometer output vector f attitude _b@[f _Bxf _Byf _Bz] ^T, gravity acceleration g, system's roll angle estimated value γ _D, angle of pitch estimated values theta _DWith course angle estimated value ψ _DRelation between (intermediate variable):

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]\&ap;\left[\begin{array}{ccc}{\cos \mathrm{\&psi;}}_D{\cos \mathrm{\&theta;}}_D& {\sin \mathrm{\&psi;}}_D{\cos \mathrm{\&theta;}}_D& -\sin {\mathrm{\&theta;}}_D\\ {\cos \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\sin \mathrm{\&gamma;}}_D-{\sin \mathrm{\&psi;}}_D{\cos \mathrm{\&gamma;}}_D& {\sin \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\sin \mathrm{\&gamma;}}_D+{\cos \mathrm{\&psi;}}_D{\cos \mathrm{\&gamma;}}_D& {\cos \mathrm{\&theta;}}_D{\sin \mathrm{\&gamma;}}_D\\ {\cos \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\cos \mathrm{\&gamma;}}_D+{\sin \mathrm{\&psi;}}_D{\sin \mathrm{\&gamma;}}_D& {\sin \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\cos \mathrm{\&gamma;}}_D-{\cos \mathrm{\&psi;}}_D{\sin \mathrm{\&gamma;}}_D& {\cos \mathrm{\&theta;}}_D\cos {\mathrm{\&gamma;}}_{\mathrm{<figure-callout\; id="0"\; label="D"\; filenames="A20061004066200203.png,A20061004066200211.png"\; state="\{\{state\}\}">D</figure-callout>}}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]---\left(1\right)$

Thereby can obtain:

$\left\{\begin{array}{c}{f}_{\mathrm{bx}}={\sin \mathrm{\&theta;}}_D\&CenterDot;g\\ f_{\mathrm{by}}=-{\cos \mathrm{\&theta;}}_D\&CenterDot;{\sin \mathrm{\&gamma;}}_D\&CenterDot;g\\ f_{\mathrm{bz}}=-{\cos \mathrm{\&theta;}}_D\&CenterDot;{\cos \mathrm{\&gamma;}}_D\&CenterDot;g\end{array}\right.---\left(2\right)$

Thus, can obtain the formula that the internal damping attitude estimates is:

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_D=\arcsin \left(\frac{f_{\mathrm{<figure-callout\; id="2"\; label="bx\; f\; bx"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">bx</figure-callout>}}}{\sqrt{f_{\mathrm{bx}}^{}}}\right)\\ {\mathrm{\&gamma;}}_D=\left\{\begin{array}{c}\arctan (f_{\mathrm{by}}/f_{\mathrm{bz}});\left|f_{\mathrm{bz}}\right|\&GreaterEqual;0.1g\\ \arccos (-\frac{f_{\mathrm{bz}}}{{\cos \mathrm{\&theta;}}_D\&CenterDot;\sqrt{{\mathrm{<figure-callout\; id="2"\; label="f\; bx"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">f</figure-callout>}}_{\mathrm{bx}}^{}+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}});\left|f_{\mathrm{bz}}\right|<0.1g\end{array}\right.\end{array}\right.---\left(3\right)$

(4) confidence level of judgement attitude estimated value:

Utilize following formula (2) to obtain the accelerometer measures value transform to geographic coordinate system earlier, judge the situation of system's accelerated motion, determine the confidence level of the attitude estimated value that obtains according to formula (3) again; The pacing items that system's attitude estimation technique use is set is:

In the formula, f _NxAnd f _NySystem level acceleration under the expression geographic coordinate system.X ₁X ₂Y ₁Y ₂Expression system respectively uses the threshold value of attitude estimated value;

(5) carrying out attitude information merges:

If the attitude estimated value of the formula (3) that obtains according to formula (4) is believable, the structure that then carries out Kalman filter carries out attitude information and merges, otherwise, leap to step (6); This step comprises the steps:

1. set up the state equation of Kalman filtering:

It is as follows to use the first-order linear stochastic differential equation to describe the state error of strapdown attitude system:

In the formula, the state vector of etching system when X (t) is t; A (t), G (t) is respectively system state matrix and noise matrix; W (t) and N (t) are respectively the noise vector and the measurement noise vector of system;

The state vector of system is:

X＝[φ _n?φ _e?φ _d?δv _n?δv _e?ε _rx?ε _ry?ε _rz?] ^T (6)

The white noise vector of system is:

W＝[ω _gx?ω _gy?ω _gz?ω _rx?ω _ry?ω _rz?ω _ax?ω _ay?ω _az] ^T (7)

Wherein, φ _nφ _eφ _dRepresent respectively the north orientation, east orientation of system and ground to attitude error; δ v _nδ v _eThe velocity error of representing the north orientation and the east orientation of system respectively; ε _Rxε _Ryε _RzThe error of representing X, Y, Z axle gyro respectively; ω _Gxω _Gyω _GzBut the single order horse husband process of representing X, Y, Z axle gyroscope error model respectively; ω _Rxω _Ryω _RzThe white noise error of representing X, Y, Z axle gyroscope error model respectively; ω _Axω _Ayω _AzThe white noise error of representing X, Y, Z axis accelerometer error model respectively;

The system noise factor matrix is:

$G\left(t\right)={\left[\begin{array}{ccc}{-C}_b^n& O_{3\&times;3}& O_{3\&times;3}\\ {\mathrm{<figure-callout\; id="2"\; label="O"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">O</figure-callout>}}_{2\&times;3}& {\mathrm{<figure-callout\; id="2"\; label="O"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">O</figure-callout>}}_{2\&times;3}& {\mathrm{<figure-callout\; id="2"\; label="C\; b"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">C</figure-callout>}}_b^{2\&times;3}\\ O_{3\&times;3}& I_{3\&times;3}& O_{3\&times;3}\end{array}\right]}_{8\&times;9}---\left(8\right)$

The state transitions battle array of system is:

$A\left(t\right)={\left[\begin{array}{cc}{\left(A_{\mathrm{INS}}\right)}_{5\&times;5}& {\left(A_S\right)}_{5\&times;3}\\ O_{3\&times;5}& {\left(A_{\mathrm{IMU}}\right)}_{3\&times;3}\end{array}\right]}_{8\&times;8}---\left(9\right)$

In the following formula,

$A_{\mathrm{IMU}}=\mathrm{diag}\left[\begin{array}{ccc}-\frac{1}{T_{\mathrm{rx}}}& -\frac{1}{T_{\mathrm{ry}}}& -\frac{1}{T_{\mathrm{rz}}}\end{array}\right]---\left(10\right)$

$A_S={\left[\begin{array}{c}{-C}_{\mathrm{<figure-callout\; id="2"\; label="b\; n\; O"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">b</figure-callout>}}^n& \\ O_{}{}_{2\&times;3}\end{array}\right]}_{5\&times;3}---\left(11\right)$

A _INSBe the matrix of corresponding 5 basic navigation parameters, its nonzero term element is:

$\left\{\begin{array}{cc}A\left(\mathrm{1,3}\right)=\frac{v_N}{R_M+h}& A\left(\mathrm{1,5}\right)=\frac{1}{R_N+h}\\ A(2,3)={\mathrm{\&omega;}}_{\mathrm{ie}}+\frac{v_E}{R_N+h}& A(2,4)=-\frac{1}{R_M+h}\\ A(3,1)=-\frac{v_N}{R_M+h}& A\left(\mathrm{3,2}\right)=-{\mathrm{\&omega;}}_{\mathrm{ie}}-\frac{v_E}{R_N+h}\\ A(4,2)=-f_D& {A\left(\mathrm{4,3}\right)=f}_E\\ A(5,1)=f_D& {A\left(\mathrm{5,3}\right)=f}_E\end{array}\right.---\left(12\right)$

To the white noise vector of 8 scalariform attitude equation correspondences, get white noise variance battle array and be:

$Q\left(t\right)=\mathrm{diag}\left[\begin{array}{ccccccccc}{\mathrm{\&sigma;}}_{\mathrm{<figure-callout\; id="2"\; label="gx"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">gx</figure-callout>}}^2& {\mathrm{\&sigma;}}_{\mathrm{<figure-callout\; id="2"\; label="gy"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">gy</figure-callout>}}^2& {\mathrm{\&sigma;}}_{\mathrm{<figure-callout\; id="2"\; label="gz"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">gz</figure-callout>}}^2& \frac{{2\mathrm{\&beta;}}_x{\mathrm{\&sigma;}}_{\mathrm{rx}}^2}{{\mathrm{<figure-callout\; id="2"\; label="T\; kf"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">T</figure-callout>}}_{\mathrm{kf}}}& \frac{{2\mathrm{\&beta;}}_y{\mathrm{\&sigma;}}_{\mathrm{ry}}^2}{{\mathrm{<figure-callout\; id="2"\; label="T\; kf"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">T</figure-callout>}}_{\mathrm{kf}}}& \frac{{2\mathrm{\&beta;}}_z{\mathrm{\&sigma;}}_{\mathrm{rz}}^2}{T_{\mathrm{kf}}}& {\mathrm{\&sigma;}}_{\mathrm{<figure-callout\; id="2"\; label="ax"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">ax</figure-callout>}}^2& {\mathrm{\&sigma;}}_{\mathrm{ay}}^2& {\mathrm{\&sigma;}}_{\mathrm{az}}^2\end{array}\right]---\left(13\right)$

Wherein, σ _Gx, σ _Gy, σ _GzBe the white noise drift root-mean-square value of gyro, σ _Rx, σ _Ry, σ _RzBut be the single order horse husband of the gyro root-mean-square value that drifts about, β _x, β _x, β _xBe correlated frequency, σ _Ax, σ _Ay, σ _AzBe accelerometer white noise root-mean-square value, T _KfIt is the cycle length of discretize;

2. set up the observation equation of Kalman filtering:

Use the first-order linear stochastic differential equation to describe the measuring error of boat appearance system, its equation is as follows:

$Z\left(t\right)@\left[\begin{array}{c}{\mathrm{\&theta;}}_I-{\mathrm{\&theta;}}_D\\ {\mathrm{\&gamma;}}_I-{\mathrm{\&gamma;}}_D\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&delta;\&theta;}}_I+{\mathrm{\&delta;\&theta;}}_D\\ {\mathrm{\&delta;\&gamma;}}_I+{\mathrm{\&delta;\&gamma;}}_D\end{array}\right]@H\left(t\right)X\left(t\right)+N\left(t\right)---\left(13\right)$

In the formula, the measurement vector of etching system when Z (t) is t; H (t) is the system measurements matrix; N (t) is the measurement noise vector of system.θ, γ represent system's true horizon attitude angle, θ _D, γ _DThe attitude angle that expression is estimated by step 3, δ θ _D, δ γ _DThe error angle of expression attitude estimated value.

The system measurements matrix is:

H(t)＝[I _2×2 O _2×6] (14)

Though the measurement noise of internal damping attitude and system's acceleration, speed, latitude all have relation, the internal damping attitude error after over-compensation does not present tangible linear dependence, so be approximately the white noise vector:

N(t)＝[M _N?M _E] ^T (15)

M in the formula _NM _EThe white noise error of representing north orientation and east orientation estimation attitude respectively.

(6) output navigational parameter: navigational computer form and speed according to the rules is transferred to control corresponding unit or display instrument with the navigational parameter that calculates.

Be provided with in the above-mentioned step (4) pacing items that the system attitude estimation technique uses also can for:

In the formula, f _Bxf _Byf _BzThe output valve of expression three axis accelerometer; f _NzSystem's normal acceleration under the expression geographic coordinate system, X ₁X ₂Y ₁Y ₂Expression system respectively uses the threshold value of attitude estimated value.Simultaneously, the pacing items that system's attitude estimation technique is used also can be set according to actual needs.

Hardware comprises that the six degree of freedom inertial measurement cluster (is called for short IMU in the strapdown inertial navigation system that the present invention uses; Comprise three orthogonal gyros and three orthogonal accelerometers), navigational computer forms as shown in Figure 1 with relevant control display device, particular hardware; In the basic composition element of strapdown inertial navigation system, IMU must be installed in the geometric center position of carrier, link to each other with navigational computer by data line, navigational computer outputs to control corresponding device or display device with navigational parameter, Figure 2 shows that with the aircraft to be the system hardware scheme of installation of example; Figure 3 shows that with the trailer bus to be the system hardware scheme of installation of example.

3, beneficial effect; Method of the present invention has the following advantages: (1) does not increase any hardware cost and does not change the hardware mounting structure; (2) have independence completely, be not subjected to the influence of external environment, can all weather operations; (3) can effectively improve the navigation accuracy, particularly attitude accuracy of system; (4) system can for control display device provide p.s. the navigation signal more than 50 times.

Beneficial effect of the present invention is described as follows:

(1) the inertia components and parts precision that adopts according to envelope test is average as follows with hundred seconds average IMU accuracy tables to ten seconds respectively under 0.02 second the sample frequency:

Ten seconds average and hundred seconds average IMU standard deviations of table 1

Utilize the IMU of above-mentioned precision to build the system principle model machine, then IMU is placed on the turntable of level, carry out 3 hours static horizontal test, test findings as shown in Figure 7 and Figure 8.Utilize the method for the invention to obtain new strapdown inertial navigation system roll angle curve shown in Fig. 7 (a) and Fig. 8 (a).For method of the present invention and traditional strap-down inertial control method are compared, utilize same group of IMU data file to carry out traditional pure strapdown and resolve, obtain correlation curve such as Fig. 7 (b) and Fig. 8 (b) of attitude.By Fig. 7 and Fig. 8 as can be known, under the static horizontal situation, when only utilizing gyro and accelerometer information, the attitude angle of utilizing traditional strapdown inertial navigation system calculation method to obtain has tangible drift to disperse phenomenon, and has tangible shura periodic oscillations characteristic, and adopt method of the present invention can effectively suppress the drift and the vibration of system's attitude angle, and the static attitude precision has been brought up in 0.05 °.

(2) test on three-axle table:

The IMU precision of using is with table 1.

1) roll angle rotates test:

IMU is placed on the three-axle table leveling turntable.Initial time turntable level increases by 5 ° or 10 ° every two fens kind turntable inside casings.New strapdown inertial navigation system roll angle curve is shown in Fig. 9 (a).For new system and traditional strap-down inertial are compared, utilize same group of IMU data file to carry out traditional pure strapdown and resolve, obtain correlation curve figure such as Fig. 9 (b) of system's angle of pitch;

2) angle of pitch rotates test:

IMU is placed on the three-axle table, with leveling turntable.Initial time turntable level makes the turntable center reduce 5 ° or 10 ° every two fens kinds.Obtain new strapdown inertial navigation system angle of pitch curve shown in Figure 10 (a).For new system and traditional strap-down inertial are compared, utilize same group of IMU data file to carry out traditional pure strapdown and resolve, obtain correlation curve figure such as Figure 10 (b) of system's angle of pitch;

3) roll angle rolling test:

IMU is placed on the horizontal three-axle table, and turntable keeps horizontal stationary after 7 minutes, and it is 0.1Hz that the turntable inside casing carries out frequency, and amplitude is ± 10 ° oscillating motion.Obtain new strapdown inertial navigation system roll angle curve shown in Figure 11 (a).System utilizes same group of IMU data file to carry out traditional pure strapdown and resolves, and obtains roll angle curve such as Figure 11 (b);

4) angle of pitch circumference test

IMU is placed on the horizontal three-axle table, and initial time turntable level, stationary state were made back turntable center in 7 minutes and are carried out the circular motion that speed is 20 °/s, obtained new strapdown inertial navigation system angle of pitch curve shown in Figure 12 (a).In order obviously to see kinetic characteristic, provide angle of pitch curve map such as Figure 12 (b) in a certain period.

The experiment of system on three-axle table shows, when system has certain attitude angle, when having certain angular velocity or angular acceleration, adopts acceleration to carry out the attitude accuracy of the strapdown inertial navigation system that attitude estimates apparently higher than traditional strapdown inertial navigation system precision.

(3) preventing test of system

Dynamically preventing test carries out on one section comparatively straight highway.The attitude angle approximate horizontal of vehicle, course remain 330 ° substantially.Vehicle is initially static, after accelerated motion in two minutes, keeps the speed of 36km/h substantially, and experimental period is 400s.The data that provide with an other cover high-precision integrated navigation system on the vehicle are reference, obtained system's boat appearance error angle in the sport car process, resolve with method of the present invention and traditional strapdown inertial navigation system algorithm respectively, obtain roll angle correlation curve such as Figure 13 (a) of system, obtain angle of pitch correlation curve shown in Figure 13 (b).

Dynamically the preventing test result shows, (0～100s), the acceleration estimation attitude is unavailable, when system enters the at the uniform velocity smooth-ride stage (100～400s) under the accelerated condition when system is in, system adopts the attitude value of acceleration estimation, and the attitude error of system has been reduced one times.A large amount of dynamic experiments show, if system can remain a constant speed in long-time, promptly use the time of accelerometer attitude estimated value long more, and then the raising of system accuracy just is obviously more.

Four, description of drawings

Fig. 1 is the strapdown inertial navigation system hardware block diagram;

Fig. 2 is a system hardware scheme of installation in the aircraft;

Fig. 3 is a system hardware scheme of installation in the trailer bus;

Fig. 4 is traditional strapdown inertial navigation system schematic diagram;

Fig. 5 is a strapdown inertial navigation system schematic diagram of the present invention;

Fig. 6 is novel strapdown inertial navigation system software flow pattern;

Fig. 7 is a static system roll curve map

Fig. 8 is a static system pitching curve map;

Fig. 9 is a roll angle position measurement curve map;

Figure 10 is pitch position test curve figure;

Figure 11 waves test curve figure for roll angle;

Figure 12 is angle of pitch circumference test curve figure;

Figure 13 is a dynamic sport car system attitude curve map;

Figure 14 is novel integrated navigation system software flow pattern.

Five, embodiment

Embodiment one:

Strapdown inertial navigation system hardware is formed identical with traditional strapdown inertial navigation system hardware composition in the present embodiment.As shown in Figure 5, present embodiment comprises the following steps:

(1) utilize sensor sensing carrier movement in the six degree of freedom inertial measurement cluster (be called for short IMU): IMU by the responsive motion carrier of gyroscope along its axial angular velocity signal, by accelerometer measures along carrier shaft to the linear acceleration signal, and signal is transferred to navigational computer;

(2) carrying out strap down inertial navigation resolves: navigational computer carries out attitude algorithm to the angular velocity signal of gyroscope sensitivity, attitude matrix is carried out trigonometric function calculate attitude angle and the position angle that promptly extracts carrier, acceleration with accelerometer measures carries out being calculated by carrier coordinate system to the coordinate transform of geographic coordinate system, and then carries out navigational parameters such as speed, position and calculate in geographic coordinate system;

(3) when carrying out step (2), carry out this step, utilize the attitude signal of acceleration estimation system: according to the relation between three axis accelerometer output, geographic coordinate system and the carrier coordinate system, when system's acceleration is very little, ignore the influence of speed and acceleration, obtain accelerometer output vector f attitude _b@[f _Bxf _Byf _Bz] ^T, gravity acceleration g, system's roll angle estimated value γ _D, angle of pitch estimated values theta _DWith course angle estimated value ψ _DBetween relation:

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]\&ap;\left[\begin{array}{ccc}{\cos \mathrm{\&psi;}}_D{\cos \mathrm{\&theta;}}_D& {\sin \mathrm{\&psi;}}_D{\cos \mathrm{\&theta;}}_D& -\sin {\mathrm{\&theta;}}_D\\ {\cos \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\sin \mathrm{\&gamma;}}_D-{\sin \mathrm{\&psi;}}_D{\cos \mathrm{\&gamma;}}_D& {\sin \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\sin \mathrm{\&gamma;}}_D+{\cos \mathrm{\&psi;}}_D{\cos \mathrm{\&gamma;}}_D& {\cos \mathrm{\&theta;}}_D{\sin \mathrm{\&gamma;}}_D\\ {\cos \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\cos \mathrm{\&gamma;}}_D+{\sin \mathrm{\&psi;}}_D{\sin \mathrm{\&gamma;}}_D& {\sin \mathrm{\&psi;}}_D{\sin \mathrm{\&theta;}}_D{\cos \mathrm{\&gamma;}}_D-{\cos \mathrm{\&psi;}}_D{\sin \mathrm{\&gamma;}}_D& {\cos \mathrm{\&theta;}}_D\cos {\mathrm{\&gamma;}}_{\mathrm{<figure-callout\; id="0"\; label="D"\; filenames="A20061004066200203.png,A20061004066200211.png"\; state="\{\{state\}\}">D</figure-callout>}}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]---\left(1\right)$

Thereby can obtain:

$\left\{\begin{array}{c}{f}_{\mathrm{bx}}={\sin \mathrm{\&theta;}}_D\&CenterDot;g\\ f_{\mathrm{by}}=-{\cos \mathrm{\&theta;}}_D\&CenterDot;{\sin \mathrm{\&gamma;}}_D\&CenterDot;g\\ f_{\mathrm{bz}}=-{\cos \mathrm{\&theta;}}_D\&CenterDot;{\cos \mathrm{\&gamma;}}_D\&CenterDot;g\end{array}\right.---\left(2\right)$

Thus, can obtain the formula that the internal damping attitude estimates is:

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_D=\arcsin \left(\frac{f_{\mathrm{<figure-callout\; id="2"\; label="bx\; f\; bx"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">bx</figure-callout>}}}{\sqrt{f_{\mathrm{bx}}^{}}}\right)\\ {\mathrm{\&gamma;}}_D=\left\{\begin{array}{c}\arctan (f_{\mathrm{by}}/f_{\mathrm{bz}});\left|f_{\mathrm{bz}}\right|\&GreaterEqual;0.1g\\ \arccos (-\frac{f_{\mathrm{bz}}}{{\cos \mathrm{\&theta;}}_D\&CenterDot;\sqrt{{\mathrm{<figure-callout\; id="2"\; label="f\; bx"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">f</figure-callout>}}_{\mathrm{bx}}^{}+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}});\left|f_{\mathrm{bz}}\right|<0.1g\end{array}\right.\end{array}\right.---\left(3\right)$

(4) confidence level of judgement attitude estimated value:

Utilize following formula (2) to obtain the accelerometer measures value transform to geographic coordinate system earlier, judge the situation of system's accelerated motion, determine the confidence level of the attitude estimated value that obtains according to formula (3) again; The pacing items that system's attitude estimation technique use is set is:

In the formula, f _NxAnd f _NySystem level acceleration under the expression geographic coordinate system.X ₁X ₂Y ₁Y ₂Expression system respectively uses the threshold value of attitude estimated value;

(5) carrying out attitude information merges:

If the attitude estimated value of the formula (3) that obtains according to formula (4) is believable, the structure that then carries out Kalman filter carries out attitude information and merges, otherwise, leap to step (6); This step comprises the steps:

1. set up the state equation of Kalman filtering:

It is as follows to use the first-order linear stochastic differential equation to describe the state error of strapdown attitude system:

X ^&(t)8×1＝A(t) _8×8X(t)+G(t) _8×9W(t) _9×1 (5)

In the formula, the state vector of etching system when X (T) is t; A (t), G (T) is respectively system state matrix and noise matrix; W (t) and N (t) are respectively the noise vector and the measurement noise vector of system;

The state vector of system is:

X＝[φ _n?φ _e?φ _d?δv _n?δv _e?ε _rx?ε _ry?ε _rz] ^T (6)

The white noise vector of system is:

W＝[ω _gx?ω _gy?ω _gz?ω _rx?ω _ry?ω _rz?ω _ax?ω _ay?ω _az] ^T (7)

Wherein, φ _nφ _eφ _dRepresent respectively the north orientation, east orientation of system and ground to attitude error; δ _Vnδ _VeThe velocity error of representing the north orientation and the east orientation of system respectively; ε _Rxε _Ryε _RzThe error of representing X, Y, Z axle gyro respectively; ω _Gxω _Gyω _GzBut the single order horse husband process of representing X, Y, Z axle gyroscope error model respectively; ω _Rxω _Ryω _RzThe white noise error of representing X, Y, Z axle gyroscope error model respectively; ω _Axω _Ayω _AzThe white noise error of representing X, Y, Z axis accelerometer error model respectively.

The system noise factor matrix is:

$G\left(t\right)={\left[\begin{array}{ccc}{-C}_b^n& O_{3\&times;3}& O_{3\&times;3}\\ {\mathrm{<figure-callout\; id="2"\; label="O"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">O</figure-callout>}}_{2\&times;3}& {\mathrm{<figure-callout\; id="2"\; label="O"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">O</figure-callout>}}_{2\&times;3}& {\mathrm{<figure-callout\; id="2"\; label="C\; b"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">C</figure-callout>}}_b^{2\&times;3}\\ O_{3\&times;3}& I_{3\&times;3}& O_{3\&times;3}\end{array}\right]}_{8\&times;9}---\left(8\right)$

The state transitions battle array of system is:

$A\left(t\right)={\left[\begin{array}{cc}{\left(A_{\mathrm{INS}}\right)}_{5\&times;5}& {\left(A_S\right)}_{5\&times;3}\\ O_{3\&times;5}& {\left(A_{\mathrm{IMU}}\right)}_{3\&times;3}\end{array}\right]}_{8\&times;8}---\left(9\right)$

In the following formula,

$A_{\mathrm{IMU}}=\mathrm{diag}\left[\begin{array}{ccc}-\frac{1}{T_{\mathrm{rx}}}& -\frac{1}{T_{\mathrm{ry}}}& -\frac{1}{T_{\mathrm{rz}}}\end{array}\right]---\left(10\right)$

$A_S={\left[\begin{array}{c}{-C}_{\mathrm{<figure-callout\; id="2"\; label="b\; n\; O"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">b</figure-callout>}}^n& \\ O_{}{}_{2\&times;3}\end{array}\right]}_{5\&times;3}---\left(11\right)$

AINS is the matrix of corresponding 5 basic navigation parameters, and its nonzero term element is:

$\left\{\begin{array}{cc}A\left(\mathrm{1,3}\right)=\frac{v_N}{R_M+h}& A\left(\mathrm{1,5}\right)=\frac{1}{R_N+h}\\ A(2,3)={\mathrm{\&omega;}}_{\mathrm{ie}}+\frac{v_E}{R_N+h}& A(2,4)=-\frac{1}{R_M+h}\\ A(3,1)=-\frac{v_N}{R_M+h}& A\left(\mathrm{3,2}\right)=-{\mathrm{\&omega;}}_{\mathrm{ie}}-\frac{v_E}{R_N+h}\\ A(4,2)=-f_D& {A\left(\mathrm{4,3}\right)=f}_E\\ A(5,1)=f_D& {A\left(\mathrm{5,3}\right)=f}_E\end{array}\right.---\left(12\right)$

To the white noise vector of 8 scalariform attitude equation correspondences, get white noise variance battle array and be:

$Q\left(t\right)=\mathrm{diag}\left[\begin{array}{ccccccccc}{\mathrm{\&sigma;}}_{\mathrm{<figure-callout\; id="2"\; label="gx"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">gx</figure-callout>}}^2& {\mathrm{\&sigma;}}_{\mathrm{<figure-callout\; id="2"\; label="gy"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">gy</figure-callout>}}^2& {\mathrm{\&sigma;}}_{\mathrm{gz}}^2& \frac{{2\mathrm{\&beta;}}_x{\mathrm{\&sigma;}}_{\mathrm{rx}}^2}{{\mathrm{<figure-callout\; id="2"\; label="T\; kf"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">T</figure-callout>}}_{\mathrm{kf}}}& \frac{{2\mathrm{\&beta;}}_y{\mathrm{\&sigma;}}_{\mathrm{ry}}^2}{{\mathrm{<figure-callout\; id="2"\; label="T\; kf"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">T</figure-callout>}}_{\mathrm{kf}}}& \frac{{2\mathrm{\&beta;}}_z{\mathrm{\&sigma;}}_{\mathrm{rz}}^2}{T_{\mathrm{kf}}}& {\mathrm{\&sigma;}}_{\mathrm{<figure-callout\; id="2"\; label="ax"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">ax</figure-callout>}}^2& {\mathrm{\&sigma;}}_{\mathrm{<figure-callout\; id="2"\; label="ay"\; filenames="A20061004066200203.png,A20061004066200231.png"\; state="\{\{state\}\}">ay</figure-callout>}}^2& {\mathrm{\&sigma;}}_{\mathrm{az}}^2\end{array}\right]---\left(13\right)$

Wherein, σ _Gx, σ _Gy, σ _GzBe the white noise drift root-mean-square value of gyro, σ _Rx, σ _Ry, σ _RzBut be the single order horse husband of the gyro root-mean-square value that drifts about, β _x, β _x, β _xBe correlated frequency, σ _Ax, σ _Ay, σ _AzBe accelerometer white noise root-mean-square value, T _KfIt is the cycle length of discretize.

2. set up the observation equation of Kalman filtering:

Use the first-order linear stochastic differential equation to describe the measuring error of boat appearance system, its equation is as follows:

$Z\left(t\right)@\left[\begin{array}{c}{\mathrm{\&theta;}}_I-{\mathrm{\&theta;}}_D\\ {\mathrm{\&gamma;}}_I-{\mathrm{\&gamma;}}_D\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&delta;\&theta;}}_I+{\mathrm{\&delta;\&theta;}}_D\\ {\mathrm{\&delta;\&gamma;}}_I+{\mathrm{\&delta;\&gamma;}}_D\end{array}\right]@H\left(t\right)X\left(t\right)+N\left(t\right)---\left(13\right)$

In the formula, the measurement vector of etching system when Z (t) is t; H (t) is the system measurements matrix; N (t) is the measurement noise vector of system.θ, γ represent system's true horizon attitude angle, θ _D, γ _DThe attitude angle that expression is estimated by step 3, δ θ _D, δ γ _DThe error angle of expression attitude estimated value.

The system measurements matrix is:

H(t)＝[I _2×2?O _2×6] (14)

Though the measurement noise of internal damping attitude and system's acceleration, speed, latitude all have relation, the internal damping attitude error after over-compensation does not present tangible linear dependence, so be approximately the white noise vector:

N(t)＝[M _N?M _E] ^T (15)

M in the formula _NM _EThe white noise error of representing north orientation and east orientation estimation attitude respectively.

(6) output navigational parameter: navigational computer form and speed according to the rules is transferred to control corresponding unit or display instrument with the navigational parameter that calculates.

As shown in Figure 6, the workflow of present embodiment is as follows: present embodiment only has the IMU sensor signal earlier pending:

(1) system initialization module:

SINS generally should normally start under static condition.At first carry out the initialization of system, this comprises the navigational computer hardware check, the initialization of each interface of interface expansion board, detection to the navigation sensor signal, and the communication between the control display, corresponding state setting, and the initialization of Kalman filter or the like.

(2) initial alignment:

Under quiet pedestal, SINS adopts two kinds of initial alignment methods: the one, by controlling the initial attitude that display directly provides system; The 2nd, after receiving the control signal and initial information that the external world provides, carry out autoregistration.According to external condition, can freely select the initial alignment method for use

(3) strapdown Inertial Attitude is resolved:

Strapdown attitude is resolved the part that module is an outbalance, mainly comprises following submodule;

1) acquisition process of inertial measuring unit data.Mainly be the signal of gathering gyroscope and accelerometer, under the situation of conditions being possessed, go back signals such as collecting temperature.

2) coefficient and coordinate conversion.The output valve of the inertia device that collects need multiply by corresponding calibration factor and just can obtain physical quantity corresponding.In addition, because the coordinate system of inertia device and actual navigation coordinate system may be inconsistent,, the physical quantity of inertia device is converted into the physical quantity of geographic coordinate system so also need to carry out certain coordinate transform.

3) find the solution hypercomplex number attitude equation.According to the hypercomplex number method in the attitude algorithm, utilize the relation between hypercomplex number and the attitude of carrier, ask for the attitude battle array.

4) compare force transformation.Specific force is an outer acting force corresponding on the unit mass, is acceleration and gravitational acceleration poor in relative inertness space, also title " non-gravitational acceleration ".What accelerometer was directly measured is not the acceleration of carrier, does computing so can not directly bring the measured value of accelerometer, and it need just can bring computing into through suitable conversion.

5) extract the attitude angle.According to the attitude battle array equation that hypercomplex number is tried to achieve, therefrom extract the attitude angle information of carrier.

6) find the solution the specific force equation.Utilize the attitude angle information and the specific force that obtain, the true acceleration when setting up specific force equation acquisition carrier movement, and then the speed and the position of acquisition carrier.

(4) utilize accelerometer output to estimate the attitude module:

The IMU signal is gathered at every turn by system, the estimation of all using formula 3 to carry out system's attitude.According to being different from the principle that strapdown Inertial Attitude is resolved, obtain the estimation numerical value of the system's roll angle and the angle of pitch.

(5) attitude is estimated the confidence level judge module:

Resolve the value of the system's acceleration under the geographic coordinate system that obtains in the process according to strapdown Inertial Attitude,, judge the confidence level that system's attitude is estimated according to formula 4.

(6) attitude integrated kalman filter module:

According to formula 5～formula 15, set up Kalman filter.When judging that the attitude estimated value is credible, carry out Kalman filtering and resolve, resolve frequency and can reach 50Hz.When system judges that wave filter is insincere, skip this module, do not carry out Kalman filtering.

(7) data outputting module:

System need be according to the demand of external device, and to different interfacing equipments, output format, frequency, verification mode and mode bit that navigation information is set are set or the like.

Embodiment two: step and the step among the embodiment in the present embodiment are basic identical, and difference is that the pacing items that system's attitude estimation technique use is set in the step (4) is:

In the formula, f _Bxf _Byf _BzThe output valve of expression three axis accelerometer; f _NzSystem's normal acceleration under the expression geographic coordinate system, X ₁X ₂Y ₁Y ₂Expression system respectively uses the threshold value of attitude estimated value.

Embodiment three:

The extraneous navigation information source that possesses other when system, for example during information such as GPS, mileage gauge, atmosphere speed calculation machine or magnetic course transmitter, system still can adopt the next further raising of the attitude estimated value system attitude accuracy of accelerometer, basic step and embodiment one are basic identical, different is in the present embodiment, system can carry out data fusion with one or more outer navigation informations, thereby further improves the precision of system, to satisfy the requirement of more application scenarios.When navigation information is auxiliary outside strapdown inertial navigation system possesses, be referred to as integrated navigation system usually.Accelerometer of the present invention estimates that the attitude method also can be applied in the integrated navigation system, and its system software process flow diagram as shown in figure 14.

Claims (3)

1, a kind of method of estimating and merging based on the attitude of strapdown inertial navigation system comprises the following steps:

(1) utilize sensor sensing carrier movement characteristic in the six degree of freedom inertial measurement cluster: by the responsive motion carrier of gyroscope along its axial angular velocity signal, by accelerometer measures along carrier shaft to the linear acceleration signal, and signal is transferred to navigational computer;

(2) carrying out strap down inertial navigation resolves: navigational computer carries out attitude algorithm to the angular velocity signal of gyroscope sensitivity, attitude matrix is carried out trigonometric function calculate attitude angle and the position angle that promptly extracts carrier, acceleration with accelerometer measures carries out being calculated by carrier coordinate system to the coordinate transform of geographic coordinate system, and then carries out navigational parameters such as speed, position and calculate in geographic coordinate system;

It is characterized in that, when carrying out above-mentioned steps (2), carry out the following step:

(3) utilize the attitude signal of linear acceleration estimating system: according to the relation between three axis accelerometer output, geographic coordinate system and the carrier coordinate system, when system's acceleration is very little, ignore the influence of speed and acceleration, obtain accelerometer output vector f to attitude _b@[f _Bxf _Byf _Bz] ^T, $f_b@{\left[\begin{array}{ccc}{f}_{\mathrm{bx}}& f_{\mathrm{by}}& f_{\mathrm{bz}}\end{array}\right]}^T,$ The estimated value γ of gravity acceleration g, system's roll angle _D, angle of pitch estimated values theta _DWith course angle estimated value ψ _DRelation between (intermediate variable):

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]\&ap;\left[\begin{array}{ccc}\cos {\mathrm{\&psi;}}_D\cos {\mathrm{\&theta;}}_D& \sin {\mathrm{\&psi;}}_D\cos {\mathrm{\&theta;}}_D& -\sin {\mathrm{\&theta;}}_D\\ \cos {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\sin {\mathrm{\&gamma;}}_D& \sin {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\sin {\mathrm{\&gamma;}}_D& \\ -\sin {\mathrm{\&psi;}}_D\cos {\mathrm{\&gamma;}}_D& +\cos {\mathrm{\&psi;}}_D\cos {\mathrm{\&gamma;}}_D& \cos {\mathrm{\&theta;}}_D\sin {\mathrm{\&gamma;}}_D\\ \cos {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\cos {\mathrm{\&gamma;}}_D& \sin {\mathrm{\&psi;}}_D\sin {\mathrm{\&theta;}}_D\cos {\mathrm{\&gamma;}}_D& \\ +\sin {\mathrm{\&psi;}}_D\sin {\mathrm{\&gamma;}}_D& -\cos {\mathrm{\&psi;}}_D\sin {\mathrm{\&gamma;}}_D& \cos {\mathrm{\&theta;}}_D\cos {\mathrm{\&gamma;}}_D\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]---\left(1\right)$

Thereby can obtain:

$\left\{\begin{array}{c}{f}_{\mathrm{bx}}=\sin {\mathrm{\&theta;}}_D\&CenterDot;g\\ f_{\mathrm{by}}=-\cos {\mathrm{\&theta;}}_D\&CenterDot;\sin {\mathrm{\&gamma;}}_D\&CenterDot;g\\ f_{\mathrm{bz}}=-\cos {\mathrm{\&theta;}}_D\&CenterDot;\cos {\mathrm{\&gamma;}}_D\&CenterDot;g\end{array}\right.---\left(2\right)$

Thus, can obtain the formula that the internal damping attitude estimates is:

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_D=\arcsin \left(\frac{f_{\mathrm{bx}}}{\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}}\right)\\ {\mathrm{\&gamma;}}_D=\left\{\begin{array}{c}\arctan (f_{\mathrm{by}}/f_{\mathrm{bz}});\left|f_{\mathrm{bz}}\right|\&GreaterEqual;0.1g\\ \arccos (-\frac{f_{\mathrm{bz}}}{\cos {\mathrm{\&theta;}}_D\&CenterDot;\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}});\left|f_{\mathrm{bz}}\right|<0.1g\end{array}\right.\end{array}\right.---\left(3\right)$

(4) confidence level of judgement attitude estimated value:

Judge the situation of system's accelerated motion according to this acceleration measuring value, determine the confidence level of the attitude estimated value that obtains according to formula (3);

(5) carrying out attitude information merges:

If the attitude estimated value of the formula (3) that obtains according to formula (4) is believable, the structure that then carries out Kalman filter carries out attitude information and merges, otherwise, leap to step (6); This step comprises the steps:

1. set up the state equation of Kalman filtering:

It is as follows to use the first-order linear stochastic differential equation to describe the state error of strapdown attitude system:

X ^&(t) _8×1＝A(t) _8×8X(t)+G(t) _8×9W(t) _9×1 (5)

In the formula, the state vector of etching system when X (t) is t; A (t), G (t) is respectively system state matrix and noise matrix; W (t) is the noise vector of system;

The state vector of system is:

X＝[φ _n?φ _e?φ _d?δv _n?δv _e?ε _rx?ε _ry?ε _rz] ^T (6)

The white noise vector of system is:

W＝[ω _gx?ω _gy?ω _gz?ω _rx?ω _ry?ω _rz?ω _ax?ω _ay?ω _az] ^T (7)

Wherein, φ _nφ _eφ _dRepresent respectively the north orientation, east orientation of system and ground to attitude error; δ v _nδ v _eThe velocity error of representing the north orientation and the east orientation of system respectively; ε _Rxε _Ryε _RzThe error of representing X, Y, Z axle gyro respectively; ω _Gxω _Gyω _GzBut the single order horse husband process of representing X, Y, Z axle gyroscope error model respectively; ω _Rxω _Ryω _RzThe white noise error of representing X, Y, Z axle gyroscope error model respectively; ω _Axω _Ayω _AzThe white noise error of representing X, Y, Z axis accelerometer error model respectively;

The system noise factor matrix is:

$G\left(t\right)={\left[\begin{array}{ccc}-C_b^n& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{2\&times;3}& 0_{2\&times;3}& C_{b2\&times;3}^n\\ 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}\end{array}\right]}_{8\&times;9}---\left(8\right)$

The state transitions battle array of system is:

$A\left(t\right)={\left[\begin{array}{cc}{\left(A_{\mathrm{INS}}\right)}_{5\&times;5}& {\left(A_S\right)}_{5\&times;3}\\ 0_{3\&times;5}& {\left(A_{\mathrm{IMU}}\right)}_{3\&times;3}\end{array}\right]}_{8\&times;8}---\left(9\right)$

In the following formula,

$A_{\mathrm{IMU}}=\mathrm{diag}\left[\begin{array}{ccc}-\frac{1}{T_{\mathrm{rx}}}& -\frac{1}{T_{\mathrm{ry}}}& -\frac{1}{T_{\mathrm{rz}}}\end{array}\right]---\left(10\right)$

$A_S={\left[\begin{array}{c}-C_b^n\\ 0_{2\&times;3}\end{array}\right]}_{5\&times;3}---\left(11\right)$

A _INSBe the matrix of corresponding 5 basic navigation parameters, its nonzero term element is:

$\left\{\begin{array}{cc}A\left(\mathrm{1,3}\right)=\frac{v_N}{R_M+h}& A\left(\mathrm{1,5}\right)=\frac{1}{R_N+h}\\ A\left(\mathrm{2,3}\right)={\mathrm{\&omega;}}_{\mathrm{ie}}+\frac{v_N}{R_N+h}& A(2,4)=-\frac{1}{R_M+h}\\ A(3,1)=-\frac{v_N}{R_M+h}& A(3,2)=-{\mathrm{\&omega;}}_{\mathrm{ie}}-\frac{v_E}{R_N+h}\\ A\left(\mathrm{4,2}\right)=-f_D& A\left(\mathrm{4,3}\right)=f_E\\ A\left(\mathrm{5,1}\right)=f_D& A\left(\mathrm{5,3}\right)=-f_E\end{array}\right.---\left(12\right)$

To the white noise vector of 8 scalariform attitude equation correspondences, get white noise variance battle array and be:

$Q\left(t\right)=\mathrm{diag}\left[\begin{array}{ccccccccc}{\mathrm{\&sigma;}}_{\mathrm{gx}}^2& {\mathrm{\&sigma;}}_{\mathrm{gy}}^2& {\mathrm{\&sigma;}}_{\mathrm{gz}}^2& \frac{{2\mathrm{\&beta;}}_x{\mathrm{\&sigma;}}_{\mathrm{rx}}^2}{T_{\mathrm{kf}}}& \frac{2{\mathrm{\&beta;}}_y{\mathrm{\&sigma;}}_{\mathrm{ry}}^2}{T_{\mathrm{kf}}}& \frac{2{\mathrm{\&beta;}}_z{\mathrm{\&sigma;}}_{\mathrm{rz}}^2}{T_{\mathrm{kf}}}& {\mathrm{\&sigma;}}_{\mathrm{ax}}^2& {\mathrm{\&sigma;}}_{\mathrm{ay}}^2& {\mathrm{\&sigma;}}_{\mathrm{az}}^2\end{array}\right]---\left(13\right)$

Wherein, σ _Gx, σ _Gy, σ _GzBe the white noise drift root-mean-square value of gyro, σ _Rx, σ _Ry, σ _RzBut be the single order horse husband of the gyro root-mean-square value that drifts about, β _x, β _x, β _xBe correlated frequency, σ _Ax, σ _Ay, σ _AzBe accelerometer white noise root-mean-square value, T _KfIt is the cycle length of discretize;

2. set up the observation equation of Kalman filtering:

Use the first-order linear stochastic differential equation to describe the measuring error of boat appearance system, its equation is as follows:

$Z\left(t\right)@\left[\begin{array}{c}{\mathrm{\&theta;}}_I-{\mathrm{\&theta;}}_D\\ {\mathrm{\&gamma;}}_I-{\mathrm{\&gamma;}}_D\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&delta;\&theta;}}_I+{\mathrm{\&delta;\&theta;}}_D\\ {\mathrm{\&delta;\&gamma;}}_I+{\mathrm{\&delta;\&gamma;}}_D\end{array}\right]@H\left(t\right)X\left(t\right)+N\left(t\right)---\left(13\right)$

In the formula, the measurement vector of etching system when Z (t) is t; H (t) is the system measurements matrix; N (t) is the measurement noise vector of system.θ, γ represent system's true horizon attitude angle; θ _I, γ _IThe inertia system attitude angle that expression is calculated by step 2, δ θ _I, δ γ _IExpression inertia system attitude error angle; θ _D, γ _DThe attitude angle that expression is estimated by step 3, δ θ _D, δ γ _DThe error angle of expression attitude estimated value;

The system measurements matrix is:

H(t)＝[I _2×2?0 _2×6] (14)

Though the measurement noise of internal damping attitude and system's acceleration, speed, latitude all have relation, the internal damping attitude error after over-compensation does not present tangible linear dependence, so be approximately the white noise vector:

N(t)＝[M _N?M _E] ^T (15)

M in the formula _NM _EThe white noise error of representing north orientation and east orientation estimation attitude respectively; (6) output navigational parameter: navigational computer form and speed according to the rules is transferred to control corresponding unit or display instrument with the navigational parameter that calculates.

2, the method for estimating and merging based on the attitude of strapdown inertial navigation system as claimed in claim 1 is characterized in that, the pacing items that system's attitude estimation technique use is set in the step (4) is:

In the formula, f _NxAnd f _NySystem level acceleration under the expression geographic coordinate system, it is to obtain after system's accelerometer output valve transforms to geographic coordinate system; X ₁X ₂Y ₁Y ₂Expression system respectively uses the threshold value of attitude estimated value.

3, the method for estimating and merging based on the attitude of strapdown inertial navigation system as claimed in claim 1 is characterized in that, the pacing items that system's attitude estimation technique use is set in the step (4) is:

In the formula, f _Bxf _Byf _BzThe output valve of expression three axis accelerometer; f _NzSystem's normal acceleration under the expression geographic coordinate system, X ₁X ₂Y ₁Y ₂Not other expression systems uses the threshold value of attitude estimated value.

Images