CN105698822A - Autonomous inertial navigation action initial alignment method based on reverse attitude tracking - Google Patents

Abstract

Disclosed is an autonomous inertial navigation action initial alignment method based on reverse attitude tracking. The method comprises the following steps of 1, building a transition reference coordinate system; 2, in the carrier motion process, calculating a rough attitude matrix according to sampled data of a sensor subsystem; 3, tracking differentiator speedometer speed differential; 4, calculating an initial alignment start time attitude matrix through reverse attitude tracking by means of the stored sampled data of the sensor subsystem; 5, performing inertial navigation calculation by means of the stored sampled data of the sensor subsystem, building a Kalman filter for fine alignment, and obtaining the precise attitude matrix and carrier position information. According to the method, the sampled data of the sensor is sufficiently utilized, action precise alignment attitude matrix obtaining and position navigation can be achieved on the condition that only the initial position of the carrier is known, the carrier mobility is greatly improved, and the action initial alignment method is effective.

Description

Based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking

One, technical field

The present invention proposes based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking, and it relates to Initial Alignment Method between a kind of inertial navigation traveling, belongs to inertial navigation Initial Alignment Technique field。

Two, background technology

Inertial navigation is initially directed between advancing and refers to that carrier completes the technology that inertial navigation system is initially directed in motor process, and therefore, it is the one of initial alignment on moving base technology。Between inertial navigation is advanced, Initial Alignment Technique has immeasurable meaning and effect for the maneuverability and quick-reaction capability strengthening carrier。Therefore, how realizing initial alignment in carrier movement process is a problem being worth research。

Environment initially it is directed at different from traditional quiet pedestal, under carrier movement state, the position of carrier, speed, acceleration and angular velocity are all being constantly occurring change, the impact of initial alignment is mainly embodied in by it: on the one hand, line athletic meeting makes the parameter moment such as the acceleration over the ground in inertial navigation fundamental equation, brother's formula acceleration change, and accelerometer output data therefore cannot be utilized under kinestate to record the precise information of acceleration of gravity；On the other hand, under moving condition, carrier vibration makes disturbance angle velocity have very wide frequency band, and output from Gyroscope signal to noise ratio is relatively low, it is impossible to exports data from gyroscope and is extracted by the useful information of this alignment of rotational-angular velocity of the earth。

Visible, when carrier movement, the direct metrical information of gyroscope and accelerometer cannot be relied on merely initially to be directed at, and need to introduce range finding or the information that tests the speed, to compensate harmful acceleration impact on initial alignment precision in motor process。At present, between inertial navigation is advanced, alignment methods currently mainly has strapdown compass method, inertial system alignment methods and optimal estimation alignment methods etc.。Strapdown compass method applies the classical control theory method of maturation and realizes initially being directed between traveling, and principle is simple but the alignment time is longer and the low-frequency disturbance of gyro is comparatively sensitive, it is necessary to choose suitable control parameter according to movement environment。Inertial system alignment methods with inertial space for middle transition coordinate system, the isolation carrier angular movement interference to initial alignment, but measurement error is only done simple process by the method, thus alignment precision is not high and can not obtain the positional information of carrier。Optimal estimation alignment methods sets up inertial navigation error equation, the information that tests the speed utilizing the tachogenerators such as speedometer carries out Kalman (Kalman) filtering as measurement information, estimates the critical errors such as the misaligned angle of the platform thus realizing initially being directed between traveling。The method is based on inertial navigation linearized stability model more, it is necessary to first obtains rough initial attitude matrix in a stationary situation and just can carry out, therefore, weakens the maneuverability advantage of carrier to a certain extent。

As can be seen here, each method is respectively arranged with its feature and the suitability。In order to rough attitude matrix can be obtained in motor process, realizing again high-precision attitude to be directed at and position navigation, the present invention proposes a kind of based on alignment methods between the autonomous type inertial navigation traveling of reverse Attitude Tracking with speedometer speed sampling data merely for alignment auxiliary information。The method, when just knowing that initial position, can realize initially being directed between high-precision inertial navigation is advanced。

Three, summary of the invention

For problems of the prior art, the present invention proposes based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking。First, in carrier movement process, utilize the information that tests the speed of speedometer to carry out coarse alignment for auxiliary and obtain rough attitude matrix, preserve the sampled data of inertial measurement component (including gyroscope and accelerometer) and speedometer simultaneously。Then carry out reverse Attitude Tracking and try to achieve the initial attitude matrix being directed at start time。On this basis, utilize the inertial measurement component sampled data preserved and speedometer speed sampling data to carry out Kalman filter fine alignment finally。Finally realize high-precision attitude tracking and position navigation。

The autonomous type inertial navigation based on reverse Attitude Tracking proposed by the invention advance between Initial Alignment Method, for vehicle-mounted inertial navigation system, this system includes sensor subsystem, data memory module, coarse alignment computing module, reverse Attitude Tracking computing module and fine alignment computing module。Relation between them is: the sampled data of sensor subsystem is delivered separately to data memory module and coarse alignment computing module；Coarse alignment computing module calculates rough initial attitude matrix and passes it to reverse Attitude Tracking computing module；Reverse Attitude Tracking computing module utilizes the sensing data of data memory module to carry out reverse Attitude Tracking, and its result of calculation is passed to fine alignment computing module。The data of fine alignment computing module recycling data memory module carry out fine alignment and calculate acquisition accurately alignment attitude matrix and position navigation results。

The present invention proposes, based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking, to specifically include following steps:

Step one: set up transition reference frame；

Four important transition reference frames that whole alignment algorithm is set up are initial time inertial coodinate system (i₀System), initial time terrestrial coordinate system (e₀System), initial time navigational coordinate system (n₀System) and initial time carrier coordinate system (i_b0System), its definition is as follows:

(1) initial time inertial coodinate system (i₀System)

When initial alignment starts, OX_i0In local meridian plane, parallel with equatorial plane；OZ_i0Point to earth rotation direction of principal axis；OY_i0With OX_i0And OZ_i0Composition right-handed helix coordinate system；After initial alignment starts, i₀It is that three axles are constant relative to inertial space；

(2) initial time terrestrial coordinate system (e₀System)

Initial time terrestrial coordinate system with earth center for coordinate origin, OX_e0Local meridian direction is pointed under the line in plane；OZ_i0Point to earth rotation direction of principal axis；OY_i0With OX_i0And OZ_i0Composition right-handed helix coordinate system；This coordinate system is relative to earth surface transfixion；

(3) initial time navigational coordinate system (n₀System)

Using east-north-sky navigational coordinate system of being initially directed at start time as initial time navigational coordinate system；After initial alignment starts, this coordinate system is motionless relative to earth surface；

(4) initial time carrier coordinate system (i_b0System)

Using the right side-front-upper carrier coordinate system of being initially directed at start time as initial time carrier coordinate system；After initial alignment starts, this coordinate system is motionless relative to inertial space；

Step 2: in carrier movement process, calculates rough attitude matrix according to sensor subsystem sampled data；

With i₀System is transition reference frame, and navigational coordinate system (n system) is relative to carrier coordinate system (b system) attitude matrixCalculating be divided into two parts, it calculates shown in process such as formula (1)

$C_b^n=C_{i_0}^n\·C_b^{i_0}---\left(1\right)$

Wherein,For n system relative to i₀The transition matrix of system；For i₀It it is the transition matrix relative to b system；

A calculates matrix

By transition reference frame n₀System, e₀System, the calculating process of this matrix can be divided into again four parts, it may be assumed that

$C_{i_0}^n=C_e^n\·C_{n_0}^e\·C_{e_0}^{n_0}\·C_{i_0}^{e_0}---\left(2\right)$

In formula,For the n system transition matrix relative to geographic coordinate system (e system)；For e system relative to n₀The transition matrix of system；

For n₀System is relative to e₀The transition matrix of system；For e₀System is relative to i₀The transition matrix of system；

Owing to cannot accurately obtain carrier positions in coarse alignment module, therefore, formula (2) is approximately:

$C_{i_0}^n\≈C_{e_0}^{n_0}\·C_{i_0}^{e_0}---\left(3\right)$

Wherein,

$C_{e_0}^{n_0}=\left[\begin{array}{ccc}0& 1& 0\\ -{\mathrm{sinL}}_0& 0& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0& 0& {\mathrm{sinL}}_0\end{array}\right]---\left(4\right)$

$C_{i_0}^{e_0}=\left[\begin{array}{ccc}cos({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& sin({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& 0\\ -sin({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& cos({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& 0\\ 0& 0& 1\end{array}\right]---\left(5\right)$

In formula, L₀For being initially directed at geographic latitude during beginning；ω_ieFor rotational-angular velocity of the earth；Δ t is current time t and be initially directed at start time t_startTime difference, i.e. Δ t=t-t_start；Formula (4-5) is substituted into after in formula (3), can obtain:

$C_{i_0}^n=\left[\begin{array}{ccc}-\sin ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& \cos ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& 0\\ -{\mathrm{sinL}}_0\·\cos ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& -{\mathrm{sinL}}_0\·\sin ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0\·\cos ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& {\mathrm{cosL}}_0\·\sin ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& {\mathrm{sinL}}_0\end{array}\right]---\left(6\right)$

B calculates matrix

With i_b0System is transition reference frame, i₀It it is the transition matrix relative to b systemCalculating be divided into two parts, it may be assumed that

$C_b^{i_0}=C_{i_{b0}}^{i_0}\·C_b^{i_{b0}}---\left(7\right)$

Wherein,For i₀System is relative to i_b0The transition matrix of system；For i_b0It it is the transition matrix relative to b system；

The measurement angular velocity utilizing gyroscope can directly obtain matrix by the attitude matrix differential equation (8)

In formula, due to i_b0System and i₀It is motionless both with respect to inertial coodinate system (i system), therefore can use the angular velocity sampled data of inertial measurement componentDirectly replaceIt addition, be initially directed at start time, i_b0System overlaps with b system, so matrixIntegral and calculating initial value be unit battle array I_3×3；

MatrixCalculating need to use terrestrial gravitation acceleration character of slow drift in inertial system；Its computational methods are as follows:

i_b0System and i₀Inertial space is motionless all relatively in system, is therefore apparent from, matrixFor with constant value matrix；To bearer rateBoth sides derivation can obtain:

${\stackrel{\·}{v}}^n={\stackrel{\·}{C}}_b^n\·v^b+C_b^n\·{\stackrel{\·}{v}}^b---\left(9\right)$

Wherein,For carrier acceleration component in n system；For carrier acceleration component in b system；

Substituted in inertial navigation fundamental equation, then the rewritable form as shown in formula (10) of inertial navigation fundamental equation；

${\stackrel{\·}{C}}_b^n\·v^b+C_b^n\·{\stackrel{\·}{v}}^b+({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{in}^n)\×v^n-f^n=g^n---\left(10\right)$

Wherein, gⁿFor terrestrial gravitation acceleration component in n system；For rotational-angular velocity of the earth component in n system；For the n system angular velocity of rotation relative to i system；FⁿFor component in n system of the specific force sampled data of inertial measurement component；

The known attitude matrix differential equation againWhereinFor the b system angular velocity of rotation relative to n system。Substituted in (10), can be obtained:

$C_b^n\·{\mathrm{\ω}}_{nb}^b\×v^b+C_b^n\·{\stackrel{\·}{v}}^b+({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{in}^n)\×v^n-f^n=g^n---\left(11\right)$

In formula,Negligible, above formula is further rewritten as formula (12)；

${\stackrel{\·}{v}}^b+{\mathrm{\ω}}_{ib}^b\×v^b-f^b=g^b---\left(12\right)$

Wherein, g^bFor terrestrial gravitation acceleration component in b system；F^bSpecific force sampled data for inertial measurement component；V^bBy speedometer speed sampling dataThe tachometric survey vector constitutedReplace；AndWith speedometer tachometric survey vector differentialReplace；It should be noted that owing to containing noise in speedometer speed sampling data, direct differentiation can amplify noise and cause that alignment precision declines, it is therefore desirable to use Nonlinear Tracking Differentiator pairCarry out differential and process that to obtain noise jamming less

In order to obtain matrixRequire over g^bWith gⁿBetween important relationship formula (13)；

$C_b^{i_{b0}}\·g^b=C_{i_0}^{i_{b0}}\·C_n^{i_0}\·g^n---\left(13\right)$

In formula, terrestrial gravitation acceleration component g under b systemⁿ=[00-g]^TFor known quantity；MatrixAll can be obtained by calculating above, therefore matrixSolve and only need to construct three not vectors in the same plane according to formula (13)；

In order to construct vector, attenuating noise simultaneously, it is integrated (13) formula both sides processing, obtains:

$z_t^{i_{b0}}=C_{i_0}^{i_{b0}}\·r_t^{i_0}---\left(14\right)$

In formula,

Obtain transition matrixRequire over g^bWith gⁿBetween important relationship formula

$C_b^{i_{b0}}\·g^b=C_{i_0}^{i_{b0}}\·C_n^{i_0}\·g^n---\left(15\right)$

Wherein,All can be obtained by calculating above；Above formula both sides integration is obtained

$z_t^{i_{b0}}=C_{i_0}^{i_{b0}}\·r_t^{i_0}---\left(16\right)$

Take two moment t₁And t₂Integrated valueWithThenCan be calculated by formula (17) and obtain；

$C_{i_0}^{i_{b_0}}=\left[\begin{array}{c}{\left(z_{t_1}^{i_{b0}}\right)}^T\\ {(z_{t_1}^{i_{b0}}\×z_{t_2}^{i_{b0}})}^T\\ {(z_{t_1}^{i_{b0}}\×z_{t_2}^{i_{b0}}\×z_{t_1}^{i_{b0}})}^T\end{array}\right]\·{\left[\begin{array}{c}{\left(r_{t_1}^{i_0}\right)}^T\\ {(r_{t_1}^{i_0}\×r_{t_2}^{i_0})}^T\\ {(r_{t_1}^{i_0}\×r_{t_2}^{i_0}\×r_{t_1}^{i_0})}^T\end{array}\right]}^{-1}---\left(17\right)$

So far, in carrier movement process, utilize formula (6), (8) and (17) to calculate and obtain initially alignment finish time t_endRough attitude matrixBut this attitude matrix precision is not high, and now can not obtain carrier positions, it is therefore desirable to preserve sensor sample data to be further processed；

Step 3: Nonlinear Tracking Differentiator processes speedometer velocity differentials；

OrderThen there is following Nonlinear Tracking Differentiator

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=-r\·sign(x_1-v(t)+\frac{x_2\left|x_2\right|}{2r})\end{array}\right.---\left(18\right)$

In formula, r is Nonlinear Tracking Differentiator parameter, and v (t) is containing noisy signal to be tracked, in this case speedometer speed sampling data；Assume that η is the sampling period, then discretization computational methods are as follows；

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+\mathrm{\η}\·x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)-\mathrm{\η}\·r\·sign\left(x_1\right(k)-v(k+1)+\frac{x_2\left(k\right)\left|x_2\left(k\right)\right|}{2r})\end{array}\right.---\left(19\right)$

So far, it is possible to obtain being disturbed the speedometer velocity differentials that sound pollution degree is less；

Step 4: utilize the sensor subsystem sampled data of storage, reverse Attitude Tracking calculates initial alignment start time attitude matrix；

Reverse Attitude Tracking algorithm is the attitude matrix in order to be obtained by coarse alignmentReversely calculate back and be initially directed at start time, obtain attitude matrixTo determine for fine alignment below and position and to lay the first stone；

Represent attitude of carrier with quaternary number q, then the resolving of attitude can be obtained by quaternion differential equation (20)；

$\stackrel{\·}{q}=-\frac{1}{2}\·{\mathrm{\Ω}}_{nb}^b\·q---\left(20\right)$

Wherein,

${\mathrm{\Ω}}_{nb}^b=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]---\left(21\right)$

${\mathrm{\ω}}_{nb}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]={\mathrm{\ω}}_{ib}^b-C_n^b.({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)---\left(22\right)$

In formula,For b system relative to component in b system of the angular velocity of n system；For rotational-angular velocity of the earth component in n system；For n system relative to component in n system of the angular velocity of terrestrial coordinate system (e system)；

In the forward of attitude resolves, it it is the gyroscope sampled data of attitude quaternion q (k) and this moment k+1 utilizing a upper moment kRemove to solve the attitude quaternion q (k+1) in k+1 moment；This calculating process adopts and finishes card solving method；Finishing card solving method is the common method solving attitude quaternion with angle increment, and its three rank approximate calculation formula is:

$q(k+1)=\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{48})\·{\mathrm{\Ω}}_{nb}^b(k+1)\·T_s\]\·q\left(k\right)---\left(23\right)$

In formula, T_sFor the posture renewal cycle；

$\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}(k+1)={\mathrm{\ω}}_{nb}^b(k+1)\·T_s\\ ={\mathrm{\ω}}_{ib}^b(k+1)\·T_s-C_n^b\left(k\right)\·\[{\mathrm{\ω}}_{ie}^n\left(k\right)+{\mathrm{\ω}}_{en}^n\left(k\right)\]\·T_s\end{array}---\left(24\right)$

Δ θ (k+1)=| | Δ θ (k+1) | | (25)

And in reverse Attitude Tracking calculates, be attitude quaternion q (k+1) and the inertial measurement component angular velocity sampled data in k moment in known k+1 momentRemove to solve attitude quaternion q (k) in k moment；Therefore, formula (23) is rewritten as formula (26)；

$\begin{array}{c}q\left(k\right)={\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{48})\·{\mathrm{\Ω}}_{nb}^b(k+1)\·T_s\]}^{-1}\·q(k+1)\\ \≈{\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{\left(k\right)}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{\left(k\right)}^2}{48})\·{\mathrm{\Ω}}_{nb}^b\left(k\right)\·T_s\]}^{-1}\·q(k+1)\end{array}---\left(26\right)$

Wherein,

$\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\left(k\right)={\mathrm{\ω}}_{nb}^b\left(k\right)\·T_s\\ ={\mathrm{\ω}}_{ib}^b\left(k\right)\·T_s-C_n^b(k+1)\·\[{\mathrm{\ω}}_{ie}^n(k+1)+{\mathrm{\ω}}_{en}^n(k+1)\]\·T_s\end{array}---\left(27\right)$

After considering coarse alignment, the positional information of carrier is unknowable, thereforeApproximate value isSimultaneously in order to improve the computational accuracy of Δ θ (k), speedometer speed sampling data are used to calculateIt is as follows that it calculates process:

${\mathrm{\ω}}_{en}^n(k+1)={\left[\begin{array}{ccc}-\frac{v_{DN}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)\·{\mathrm{tanL}}_0}{R_e}\end{array}\right]}^T---\left(28\right)$

Wherein,

So far, can through type (26), (27) and (28) by attitude matrixFollow the tracks of back and be initially directed at start time, obtainThus laying a solid foundation for ensuing fine alignment；

Step 5: utilize the sensor subsystem sampled data of storage to carry out inertial navigation resolving, set up Kalman filter and carry out fine alignment, obtain accurate attitude matrix and carrier positions information；

A. inertial navigation resolves and speedometer dead reckoning

To be initially directed at the latitude L of start time₀, longitude λ₀, height h₀And reverse Attitude Tracking resultFor resolving initial value, using the sensor sample data of storage to carry out attitude, speed and position and resolve, the inertial navigation obtaining each sample point resolves position p=[L λ h]^T, inertial navigation resolve speedInertial navigation resolves attitude matrixWith speedometer DR position p_D=[L_Dλ_Dh_D]^T, calculate speed

B. filter state model is built

Using n system as the reference frame of inertial navigation, filter state model is as follows:

In formula,For Inertial Navigation Platform misalignment；δ vⁿFor inertial navigation velocity error；δ_pFor inertial navigation site error；ε is the drift of Inertial Navigating Gyro；For inertial navigation accelerometer zero is inclined；δ p_DFor speedometer DR position error；δ K_DScale factor error for speedometer；Known terrestrial equator radius R_e, matrix M₁To M₆It is respectively as follows:

$M_1=\left[\begin{array}{ccc}0& -\frac{1}{R_e}& 0\\ \frac{1}{R_e}& 0& 0\\ \frac{\tan L}{R_e}& 0& 0\end{array}\right]---\left(30\right)$

$M^{\′}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{ie}\sin L& 0& 0\\ {\mathrm{\ω}}_{ie}\cos L& 0& 0\end{array}\right];M^{\′\′}=\left[\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ 0& 0& -\frac{v_E^n}{{R_e}^2}\\ \frac{v_E^n{\sec }^{2}L}{R_e}& 0& -\frac{v_E^n\tan L}{{R_e}^2}\end{array}\right]---\left(31\right)$

M₂=M '+M " (32)

$M_3=(v^n\×)\·M_1-\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×)---\left(33\right)$

M₄=(vⁿ×)·(2M′+M″)(34)

$M_5=\left[\begin{array}{ccc}0& \frac{1}{R_e}& 0\\ \frac{\sec L}{R_e}& 0& 0\\ 0& 0& 1\end{array}\right]---\left(35\right)$

$M_6=\left[\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ \frac{v_E^n\tan L\sec L}{R_e}& 0& -\frac{v_E^n\sec L}{{R_e}^2}\\ 0& 0& 0\end{array}\right]---\left(36\right)$

Filter state model is abbreviated as

$\stackrel{\·}{X}=F\·X+G\·W---\left(37\right)$

Wherein, systematic state transfer matrix F ∈ R^19×19, its matrix is specifically as shown in formula (38)；G is system noise transfer battle array；W is system noise；

$F=\left[\begin{array}{ccccccc}-{\mathrm{\ω}}_{in}^n\×& M_1& M_2& -C_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ f^n\×& M_3& M_4& 0_{3\×3}& C_b^n& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& M_5& M_6& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ M_5\·(v_D^n\×)& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& M_6& v_D^n\\ 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×1}\end{array}\right]---\left(38\right)$

C. measurement model is built

Inertial navigation is resolved position p and speedometer DR position p_DDifference as measuring vector Z, speedometer is measured the DR position error that noise causes and is established as the measurement noise of measurement model；

Z=p-p_D=δ p-δ p_D+ V=H X+V (39)

In formula, H is for measuring transfer matrix, H=[0_3×6I_3×30_3×6-I_3×30_3×1]；V is position measurement noise；Measure vector Z=[L-L_Dλ-λ_Dh-h_D]^T；

The inertial navigation attitude matrix that the inertial navigation error correction finally estimated with wave filter resolvesObtain accurate inertial navigation attitude matrixMeanwhile, the Accurate Estimation position p of speedometer can also be obtained_D；Achieve the accurate attitude tracking of inertial navigation and position navigation。

Wherein, in step 2, described " sensor subsystem " includes inertial measurement component and speedometer。Inertial measurement component records carrier relative to inertial space angular velocity and specific force, obtains angular velocity and specific force sampled data；Bearer rate measured by speedometer, obtains bearer rate sampled data。

Pass through above step, the autonomous type inertial navigation based on reverse Attitude Tracking proposed by the invention advance between Initial Alignment Method, take full advantage of sensor sample data, when just knowing that carrier initial position, running accurate alignment attitude battle array can be realized obtain and position navigation, substantially increase carrier maneuverability, be Initial Alignment Method between a kind of effective traveling。

It is an advantage of the current invention that:

(1) present invention proposes based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking, by reverse Attitude Tracking, coarse alignment and fine alignment are organically combined, make full use of sensor subsystem sampled data, it is possible to realize accurately being directed at the acquisition of attitude matrix and position navigation；

(2) present invention proposes based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking, utilize Kalman filter fine alignment, inertial measurement component error (accelerometer bias, gyroscope drift) and speedometer scale factor error are carried out optimal estimation, it is possible to calculating for navigation afterwards provides compensating parameter；

(3) present invention proposes based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking, when just knowing that carrier is initially directed at start time position, attitude tracking and position navigation can be realized, substantially increase the mobility of carrier, have a good application prospect；

Four, accompanying drawing explanation

Fig. 1 is Initial Alignment Method structural representation between the autonomous type inertial navigation based on reverse Attitude Tracking that the present invention proposes is advanced。

Fig. 2 is the method for the invention FB(flow block)。

In figure, sequence number, symbol, code name illustrate as follows:

1 sensor subsystem 2 data memory module 3 coarse alignment computing module

4 reverse Attitude Tracking computing module 5 fine alignment computing modules

101 inertial measurement component 102 speedometers

201 data storage cells

301 coarse alignment computing units

401 reverse Attitude Tracking computing units

501 inertial navigations resolve unit 502 speedometer dead reckoning unit 503 Kalman filter unit

Angular velocity f^bSpecific forceThe bearer rate that speedometer records

The determined attitude matrix of coarse alignment computing module

The reverse determined attitude matrix of Attitude Tracking computing module

P inertial navigation resolves position p_DSpeedometer DR positionThe determined alignment attitude matrix of fine alignment

Five, detailed description of the invention

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

The present invention proposes based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking。First, in carrier movement process, utilize the information that tests the speed of speedometer to carry out coarse alignment for auxiliary and obtain rough attitude matrixPreserve the sampled data of inertial measurement component and speedometer simultaneously。Then carry out reverse Attitude Tracking and try to achieve the initial attitude matrix being directed at start timeOn this basis, utilize the inertial measurement component sampled data preserved and speedometer speed sampling data to carry out Kalman filter fine alignment finally。Finally, high-precision alignment attitude matrix is obtainedAnd realize carrier positions navigation。

Seeing Fig. 1, the present invention proposes based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking, including sensor subsystem 1, data memory module 2, coarse alignment computing module 3, reverse Attitude Tracking computing module 4 and fine alignment computing module 5。

The sampled data of sensor subsystem systemIt is delivered separately to data memory module 2 and coarse alignment computing module 3；Coarse alignment computing module 3 calculates rough initial attitude matrixAnd pass it to reverse Attitude Tracking computing module 4；Reverse Attitude Tracking computing module 4 utilizes the sensing data of data memory module 2 to carry out reverse Attitude Tracking, and by its result of calculationPass to fine alignment computing module 5。Fine alignment computing module 5 recycles the data of data memory module 2 to carry out fine alignment and calculates to obtain and be accurately directed at attitude matrixWith position navigation results。

Seeing Fig. 2, the present invention proposes, based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking, to specifically include following steps:

Step one: set up transition reference frame；

Four important transition reference frames that whole alignment algorithm is set up are initial time inertial coodinate system (i₀System), initial time terrestrial coordinate system (e₀System), initial time navigational coordinate system (n₀System) and initial time carrier coordinate system (i_b0System), its definition is as follows:

(1) initial time inertial coodinate system (i₀System)

When initial alignment starts, OX_i0In local meridian plane, parallel with equatorial plane；OZ_i0Point to earth rotation direction of principal axis；OY_i0With OX_i0And OZ_i0Composition right-handed helix coordinate system；After initial alignment starts, i₀It is that three axles are constant relative to inertial space；

(2) initial time terrestrial coordinate system (e₀System)

Initial time terrestrial coordinate system with earth center for coordinate origin, OX_e0Local meridian direction is pointed under the line in plane；OZ_i0Point to earth rotation direction of principal axis；OY_i0With OX_i0And OZ_i0Composition right-handed helix coordinate system；This coordinate system is relative to earth surface transfixion；

(3) initial time navigational coordinate system (n₀System)

Using east-north-sky navigational coordinate system of being initially directed at start time as initial time navigational coordinate system；After initial alignment starts, this coordinate system is motionless relative to earth surface；

(4) initial time carrier coordinate system (i_b0System)

Using the right side-front-upper carrier coordinate system of being initially directed at start time as initial time carrier coordinate system；After initial alignment starts, this coordinate system is motionless relative to inertial space；

Step 2: in carrier movement process, calculates rough attitude matrix according to sensor subsystem sampled data；

With i₀System is transition reference frame, and navigational coordinate system (n system) is relative to carrier coordinate system (b system) attitude matrixCalculating be divided into two parts, it calculates shown in process such as formula (1)

$C_b^n=C_{i_0}^n\·C_b^{i_0}---\left(1\right)$

Wherein,For n system relative to i₀The transition matrix of system；For i₀It it is the transition matrix relative to b system；

A calculates matrix

By transition reference frame n₀System, e₀System, the calculating process of this matrix can be divided into again four parts, it may be assumed that

$C_{i_0}^n=C_e^n\·C_{n_0}^e\·C_{e_0}^{n_0}\·C_{i_0}^{e_0}---\left(2\right)$

In formula,For the n system transition matrix relative to geographic coordinate system (e system)；For e system relative to n₀The transition matrix of system；For n₀System is relative to e₀The transition matrix of system；For e₀System is relative to i₀The transition matrix of system；

Owing to cannot accurately obtain carrier positions in coarse alignment module, therefore, formula (2) is approximately:

$C_{i_0}^n\≈C_{e_0}^{n_0}\·C_{i_0}^{e_0}---\left(3\right)$

Wherein,

$C_{e_0}^{n_0}=\left[\begin{array}{ccc}0& 1& 0\\ -{\mathrm{sinL}}_0& 0& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0& 0& {\mathrm{sinL}}_0\end{array}\right]---\left(4\right)$

$C_{i_0}^{e_0}=\left[\begin{array}{ccc}cos({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& sin({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& 0\\ -sin({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& cos({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& 0\\ 0& 0& 1\end{array}\right]---\left(5\right)$

In formula, L₀For being initially directed at geographic latitude during beginning；ω_ieFor rotational-angular velocity of the earth；Δ t is current time t and be initially directed at start time t_startTime difference, i.e. Δ t=t-t_start；Formula (4-5) is substituted into after in formula (3), can obtain:

$C_{i_0}^n=\left[\begin{array}{ccc}-\sin ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& \cos ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& 0\\ -{\mathrm{sinL}}_0\·\cos ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& -{\mathrm{sinL}}_0\·\sin ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0\·\cos ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& {\mathrm{cosL}}_0\·\sin ({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& {\mathrm{sinL}}_0\end{array}\right]---\left(6\right)$

B calculates matrix

With i_b0System is transition reference frame, i₀It it is the transition matrix relative to b systemCalculating be divided into two parts, it may be assumed that

$C_b^{i_0}=C_{i_{b0}}^{i_0}\·C_b^{i_{b0}}---\left(7\right)$

Wherein,For i₀System is relative to i_b0The transition matrix of system；For i_b0It it is the transition matrix relative to b system；

The measurement angular velocity utilizing gyroscope can directly obtain matrix by the attitude matrix differential equation (8)

${\stackrel{\·}{C}}_b^{i_{b0}}=C_b^{i_{b0}}\·\[{\mathrm{\ω}}_{i_{b0}b}^b\×\]=C_b^{i_{b0}}\·\[{\mathrm{\ω}}_{ib}^b\×\]---\left(8\right)$

In formula, due to i_b0System and i₀It is motionless both with respect to inertial coodinate system (i system), therefore can use the angular velocity sampled data of inertial measurement componentDirectly replaceIt addition, be initially directed at start time, i_b0System overlaps with b system, so matrixIntegral and calculating initial value be unit battle array I_3×3；

MatrixCalculating need to use terrestrial gravitation acceleration character of slow drift in inertial system；Its computational methods are as follows:

i_b0System and i₀Inertial space is motionless all relatively in system, is therefore apparent from, matrixFor with constant value matrix；To bearer rateBoth sides derivation can obtain:

${\stackrel{\·}{v}}^n={\stackrel{\·}{C}}_b^n\·v^b+C_b^n\·{\stackrel{\·}{v}}^b---\left(9\right)$

Wherein,For carrier acceleration component in n system；For carrier acceleration component in b system；

Substituted in inertial navigation fundamental equation, then the rewritable form as shown in formula (10) of inertial navigation fundamental equation；

${\stackrel{\·}{C}}_b^n\·v^b+C_b^n\·{\stackrel{\·}{v}}^b+({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{in}^n)\×v^n-f^n=g^n---\left(10\right)$

Wherein, gⁿFor terrestrial gravitation acceleration component in n system；For rotational-angular velocity of the earth component in n system；For the n system angular velocity of rotation relative to i system；FⁿFor component in n system of the specific force sampled data of inertial measurement component；

The known attitude matrix differential equation againWhereinFor the b system angular velocity of rotation relative to n system。Substituted in (10), can be obtained:

$C_b^n\·{\mathrm{\ω}}_{nb}^b\×v^b+C_b^n\·{\stackrel{\·}{v}}^b+({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{in}^n)\×v^n-f^n=g^n---\left(11\right)$

In formula,Negligible, above formula is further rewritten as formula (12)；

${\stackrel{\·}{v}}^b+{\mathrm{\ω}}_{ib}^b\×v^b-f^b=g^b---\left(12\right)$

Wherein, g^bFor terrestrial gravitation acceleration component in b system；F^bSpecific force sampled data for inertial measurement component；V^bBy speedometer speed sampling dataThe tachometric survey vector constitutedReplace；AndWith speedometer tachometric survey vector differentialReplace；It should be noted that owing to containing noise in speedometer speed sampling data, direct differentiation can amplify noise and cause that alignment precision declines, it is therefore desirable to use Nonlinear Tracking Differentiator pairCarry out differential and process that to obtain noise jamming less

In order to obtain matrixRequire over g^bWith gⁿBetween important relationship formula (13)；

$C_b^{i_{b0}}\·g^b=C_{i_0}^{i_{b0}}\·C_n^{i_0}\·g^n---\left(13\right)$

In formula, terrestrial gravitation acceleration component g under b systemⁿ=[00-g]^TFor known quantity；MatrixAll can be obtained by calculating above, therefore matrixSolve and only need to construct three not vectors in the same plane according to formula (13)；

In order to construct vector, attenuating noise simultaneously, it is integrated (13) formula both sides processing, obtains:

$z_t^{i_{b0}}=C_{i_0}^{i_{b0}}\·r_t^{i_0}---\left(14\right)$

In formula,

Obtain transition matrixRequire over g^bWith gⁿBetween important relationship formula

$C_b^{i_{b0}}\·g^b=C_{i_0}^{i_{b0}}\·C_n^{i_0}\·g^n---\left(15\right)$

Wherein,All can be obtained by calculating above；Above formula both sides integration is obtained

$z_t^{i_{b0}}=C_{i_0}^{i_{b0}}\·r_t^{i_0}---\left(16\right)$

Take two moment t₁And t₂Integrated valueWithThenCan be calculated by formula (17) and obtain；

$C_{i_0}^{i_{b_0}}=\left[\begin{array}{c}{\left(z_{t_1}^{i_{b0}}\right)}^T\\ {(z_{t_1}^{i_{b0}}\×z_{t_2}^{i_{b0}})}^T\\ {(z_{t_1}^{i_{b0}}\×z_{t_2}^{i_{b0}}\×z_{t_1}^{i_{b0}})}^T\end{array}\right]\·{\left[\begin{array}{c}{\left(r_{t_1}^{i_0}\right)}^T\\ {(r_{t_1}^{i_0}\×r_{t_2}^{i_0})}^T\\ {(r_{t_1}^{i_0}\×r_{t_2}^{i_0}\×r_{t_1}^{i_0})}^T\end{array}\right]}^{-1}---\left(17\right)$

So far, in carrier movement process, utilize formula (6), (8) and (17) to calculate and obtain initially alignment finish time t_endRough attitude matrixBut this attitude matrix precision is not high, and now can not obtain carrier positions, it is therefore desirable to preserve sensor sample data to be further processed；

Step 3: Nonlinear Tracking Differentiator processes speedometer velocity differentials；

OrderThen there is following Nonlinear Tracking Differentiator

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=-r\·sign(x_1-v(t)+\frac{x_2\left|x_2\right|}{2r})\end{array}\right.---\left(18\right)$

In formula, r is Nonlinear Tracking Differentiator parameter, and v (t) is containing noisy signal to be tracked, in this case speedometer speed sampling data；Assume that η is the sampling period, then discretization computational methods are as follows；

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+\mathrm{\η}\·x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)-\mathrm{\η}\·r\·sign\left(x_1\right(k)-v(k+1)+\frac{x_2\left(k\right)\left|x_2\left(k\right)\right|}{2r})\end{array}\right.---\left(19\right)$

So far, it is possible to obtain being disturbed the speedometer velocity differentials that sound pollution degree is less；

Step 4: utilize the sensor subsystem sampled data of storage, reverse Attitude Tracking calculates initial alignment start time attitude matrix；

Reverse Attitude Tracking algorithm is the attitude matrix in order to be obtained by coarse alignmentReversely calculate back and be initially directed at start time, obtain attitude matrixTo determine for fine alignment below and position and to lay the first stone；

Represent attitude of carrier with quaternary number q, then the resolving of attitude can be obtained by quaternion differential equation (20)；

$\stackrel{\·}{q}=-\frac{1}{2}\·{\mathrm{\Ω}}_{nb}^b\·q---\left(20\right)$

Wherein,

${\mathrm{\Ω}}_{nb}^b=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]---\left(21\right)$

${\mathrm{\ω}}_{nb}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]={\mathrm{\ω}}_{ib}^b-C_n^b\·({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)---\left(22\right)$

In formula,For b system relative to component in b system of the angular velocity of n system；For rotational-angular velocity of the earth component in n system；For n system relative to component in n system of the angular velocity of terrestrial coordinate system (e system)；

In the forward of attitude resolves, it it is the gyroscope sampled data of attitude quaternion q (k) and this moment k+1 utilizing a upper moment kRemove to solve the attitude quaternion q (k+1) in k+1 moment；This calculating process adopts and finishes card solving method；Finishing card solving method is the common method solving attitude quaternion with angle increment, and its three rank approximate calculation formula is:

$q(k+1)=\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{48})\·{\mathrm{\Ω}}_{nb}^b(k+1)\·T_s\]\·q\left(k\right)---\left(23\right)$

In formula, T_sFor the posture renewal cycle；

$\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}(k+1)={\mathrm{\ω}}_{nb}^b(k+1)\·T_s\\ ={\mathrm{\ω}}_{ib}^b(k+1)\·T_s-C_n^b\left(k\right)\·\[{\mathrm{\ω}}_{ie}^n\left(k\right)+{\mathrm{\ω}}_{en}^n\left(k\right)\]\·T_s\end{array}---\left(24\right)$

Δ θ (k+1)=| | Δ θ (k+1) | | (25)

And in reverse Attitude Tracking calculates, be attitude quaternion q (k+1) and the inertial measurement component angular velocity sampled data in k moment in known k+1 momentRemove to solve attitude quaternion q (k) in k moment；Therefore, formula (23) is rewritten as formula (26)；

$\begin{array}{c}q\left(k\right)={\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{48})\·{\mathrm{\Ω}}_{nb}^b(k+1)\·T_s\]}^{-1}\·q(k+1)\\ \≈{\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{\left(k\right)}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{\left(k\right)}^2}{48})\·{\mathrm{\Ω}}_{nb}^b\left(k\right)\·T_s\]}^{-1}\·q(k+1)\end{array}---\left(26\right)$

Wherein,

$\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\left(k\right)={\mathrm{\ω}}_{nb}^b\left(k\right)\·T_s\\ ={\mathrm{\ω}}_{ib}^b\left(k\right)\·T_s-C_n^b(k+1)\·\[{\mathrm{\ω}}_{ie}^n(k+1)+{\mathrm{\ω}}_{en}^n(k+1)\]\·T_s\end{array}---\left(27\right)$

After considering coarse alignment, the positional information of carrier is unknowable, thereforeApproximate value isSimultaneously in order to improve the computational accuracy of Δ θ (k), speedometer speed sampling data are used to calculateIt is as follows that it calculates process:

${\mathrm{\ω}}_{en}^n(k+1)={\left[\begin{array}{ccc}-\frac{v_{DN}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)\·{\mathrm{tanL}}_0}{R_e}\end{array}\right]}^T---\left(28\right)$

Wherein,

So far, can through type (26), (27) and (28) by attitude matrixFollow the tracks of back and be initially directed at start time, obtainThus laying a solid foundation for ensuing fine alignment；

Step 5: utilize the sensor subsystem sampled data of storage to carry out inertial navigation resolving, set up Kalman filter and carry out fine alignment, obtain accurate attitude matrix and carrier positions information；

A. inertial navigation resolves and speedometer dead reckoning

To be initially directed at the latitude L of start time₀, longitude λ₀, height h₀And reverse Attitude Tracking resultFor resolving initial value, using the sensor sample data of storage to carry out attitude, speed and position and resolve, the inertial navigation obtaining each sample point resolves position p=[L λ h]^T, inertial navigation resolve speedInertial navigation resolves attitude matrixWith speedometer DR position p_D=[L_Dλ_Dh_D]^T, calculate speed

B. filter state model is built

Using n system as the reference frame of inertial navigation, filter state model is as follows:

In formula,For Inertial Navigation Platform misalignment；δ vⁿFor inertial navigation velocity error；δ p is inertial navigation site error；ε is the drift of Inertial Navigating Gyro；For inertial navigation accelerometer zero is inclined；δ p_DFor speedometer DR position error；δ K_DScale factor error for speedometer；Known terrestrial equator radius R_e, matrix M₁To M₆It is respectively as follows:

$M_1=\left[\begin{array}{ccc}0& -\frac{1}{R_e}& 0\\ \frac{1}{R_e}& 0& 0\\ \frac{\tan L}{R_e}& 0& 0\end{array}\right]---\left(30\right)$

$M^{\′}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{ie}\sin L& 0& 0\\ {\mathrm{\ω}}_{ie}\cos L& 0& 0\end{array}\right];M^{\′\′}=\left[\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ 0& 0& -\frac{v_E^n}{{R_e}^2}\\ \frac{v_E^n{\sec }^{2}L}{R_e}& 0& -\frac{v_E^n\tan L}{{R_e}^2}\end{array}\right]---\left(31\right)$

M₂=M '+M " (32)

$M_3=(v^n\×)\·M_1-\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×)---\left(33\right)$

M₄=(vⁿ×)·(2M′+M″)(34)

$M_5=\left[\begin{array}{ccc}0& \frac{1}{R_e}& 0\\ \frac{\sec L}{R_e}& 0& 0\\ 0& 0& 1\end{array}\right]---\left(35\right)$

$M_6=\left[\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ \frac{v_E^n\tan L\sec L}{R_e}& 0& -\frac{v_E^n\sec L}{{R_e}^2}\\ 0& 0& 0\end{array}\right]---\left(36\right)$

Filter state model is abbreviated as

$\stackrel{\·}{X}=F\·X+G\·W---\left(37\right)$

Wherein, systematic state transfer matrix F ∈ R^19×19, its matrix is specifically as shown in formula (38)；G is system noise transfer battle array；W is system noise；

$F=\left[\begin{array}{ccccccc}-{\mathrm{\ω}}_{in}^n\×& M_1& M_2& -C_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ f^n\×& M_3& M_4& 0_{3\×3}& C_b^n& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& M_5& M_6& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ M_5\·(v_D^n\×)& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& M_6& v_D^n\\ 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×1}\end{array}\right]---\left(38\right)$

C. measurement model is built

Inertial navigation is resolved position p and speedometer DR position p_DDifference as measuring vector Z, speedometer is measured the DR position error that noise causes and is established as the measurement noise of measurement model；

Z=p-p_D=δ p-δ p_D+ V=H X+V (39)

In formula, H is for measuring transfer matrix, H=[0_3×6I_3×30_3×6-I_3×30_3×1]；V is position measurement noise；Measure vector Z=[L-L_Dλ-λ_Dh-h_D]^T；

The inertial navigation attitude matrix that the inertial navigation error correction finally estimated with wave filter resolvesObtain accurate inertial navigation attitude matrixMeanwhile, the Accurate Estimation position p of speedometer can also be obtained_D；Achieve the accurate attitude tracking of inertial navigation and position navigation。

Wherein, in step 2, described " sensor subsystem " includes inertial measurement component and speedometer。Inertial measurement component records carrier relative to the angular velocity of inertial space and specific force, obtains angular velocity and specific force sampled data；Bearer rate measured by speedometer, obtains bearer rate sampled data。

Pass through above step, the autonomous type inertial navigation based on reverse Attitude Tracking proposed by the invention advance between Initial Alignment Method, take full advantage of sensor sample data, when just knowing that carrier initial position, running accurate alignment attitude battle array can be realized obtain and position navigation, substantially increase carrier maneuverability, be Initial Alignment Method between a kind of effective traveling。

Claims (2)

1. one kind based on reverse Attitude Tracking autonomous type inertial navigation advance between Initial Alignment Method, it is characterised in that: it comprises the following steps:

Step one: set up transition reference frame；

Four important transition reference frames that whole alignment algorithm is set up are initial time inertial coodinate system and i₀System, initial time terrestrial coordinate system and e₀System, initial time navigational coordinate system and n₀System and initial time carrier coordinate system and i_b0System, its definition is as follows:

(1) initial time inertial coodinate system and i₀System

When initial alignment starts, OX_i0In local meridian plane, parallel with equatorial plane；OZ_i0Point to earth rotation direction of principal axis；OY_i0With OX_i0And OZ_i0Composition right-handed helix coordinate system；After initial alignment starts, i₀It is that three axles are constant relative to inertial space；

(2) initial time terrestrial coordinate system and e₀System

Initial time terrestrial coordinate system with earth center for coordinate origin, OX_e0Local meridian direction is pointed under the line in plane；OZ_i0Point to earth rotation direction of principal axis；OY_i0With OX_i0And OZ_i0Composition right-handed helix coordinate system；This coordinate system is relative to earth surface transfixion；

(3) initial time navigational coordinate system and n₀System

Using east-north-sky navigational coordinate system of being initially directed at start time as initial time navigational coordinate system；After initial alignment starts, this coordinate system is motionless relative to earth surface；

(4) initial time carrier coordinate system and i_b0System

Using the right side-front-upper carrier coordinate system of being initially directed at start time as initial time carrier coordinate system；After initial alignment starts, this coordinate system is motionless relative to inertial space；

Step 2: in carrier movement process, calculates rough attitude matrix according to sensor subsystem sampled data；

With i₀System is transition reference frame, and navigational coordinate system and n system are relative to carrier coordinate system and b system attitude matrixCalculating be divided into two parts, it calculates shown in process such as formula (1)

$C_b^n=C_{i_0}^n\·C_b^{i_0}---\left(1\right)$

Wherein,For n system relative to i₀The transition matrix of system；For i₀It it is the transition matrix relative to b system；

A calculates matrix

By transition reference frame n₀System, e₀System, the calculating process of this matrix can be divided into again four parts, it may be assumed that

$C_{i_0}^n=C_e^n\·C_{n_0}^e\·C_{e_0}^{n_0}\·C_{i_0}^{e_0}---\left(2\right)$

In formula,For the n system transition matrix relative to geographic coordinate system and e system；For e system relative to n₀The transition matrix of system；For n₀System is relative to e₀The transition matrix of system；For e₀System is relative to i₀The transition matrix of system；

Owing to cannot accurately obtain carrier positions in coarse alignment module, therefore, formula (2) is approximately:

$C_{i_0}^n\≈C_{e_0}^{n_0}\·C_{i_0}^{e_0}---\left(3\right)$

Wherein,

$C_{e_0}^{n_0}=\left[\begin{array}{ccc}0& 1& 0\\ -\sin L_0& 0& \cos L_0\\ \cos L_0& 0& \sin L_0\end{array}\right]---\left(4\right)$

$C_{i_0}^{e_0}=\left[\begin{array}{ccc}cos({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& sin({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& 0\\ -sin({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& cos({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t)& 0\\ 0& 0& 1\end{array}\right]---\left(5\right)$

In formula, L₀For being initially directed at geographic latitude during beginning；ω_ieFor rotational-angular velocity of the earth；Δ t is current time t and be initially directed at start time t_startTime difference, i.e. Δ t=t-t_start；Formula (4-5) is substituted into after in formula (3):

$C_{i_0}^n=\left[\begin{array}{ccc}-\sin \left({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t\right)& \cos \left({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t\right)& 0\\ -\sin L_0\·\cos \left({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t\right)& -\sin L_0\·\sin \left({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t\right)& \cos L_0\\ \cos L_0\·\cos \left({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t\right)& \cos L_0\·\sin \left({\mathrm{\ω}}_{ie}\·\mathrm{\Δ}t\right)& \sin L_0\end{array}\right]---\left(6\right)$

B calculates matrix

With i_b0System is transition reference frame, i₀It it is the transition matrix relative to b systemCalculating be divided into two parts, it may be assumed that

$C_b^{i_0}=C_{i_{b0}}^{i_0}\·C_b^{i_{b0}}---\left(7\right)$

Wherein,For i₀System is relative to i_b0The transition matrix of system；For i_b0It it is the transition matrix relative to b system；

The measurement angular velocity utilizing gyroscope directly obtains matrix by the attitude matrix differential equation (8)

In formula, due to i_b0System and i₀It is motionless both with respect to inertial coodinate system and i system, therefore by the angular velocity sampled data of inertial measurement componentDirectly replaceIt addition, be initially directed at start time, i_b0System overlaps with b system, so matrixIntegral and calculating initial value be unit battle array I_3×3；

MatrixCalculating need to use terrestrial gravitation acceleration character of slow drift in inertial system；Its computational methods are as follows:

i_b0System and i₀Inertial space is motionless all relatively in system, is therefore apparent from, matrixFor with constant value matrix；To bearer rateBoth sides derivation obtains:

${\stackrel{\·}{v}}^n={\stackrel{\·}{C}}_b^n\·v^b+C_b^n\·{\stackrel{\·}{v}}^b---\left(9\right)$

Wherein,For carrier acceleration component in n system；For carrier acceleration component in b system；

Substituted in inertial navigation fundamental equation, then the rewritable form as shown in formula (10) of inertial navigation fundamental equation；

${\stackrel{\·}{C}}_b^n\·v^b+C_b^n\·{\stackrel{\·}{v}}^b+({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{in}^n)\×v^n-f^n=g^n---\left(10\right)$

Wherein, gⁿFor terrestrial gravitation acceleration component in n system；For rotational-angular velocity of the earth component in n system；

For the n system angular velocity of rotation relative to i system；FⁿFor component in n system of the specific force sampled data of inertial measurement component；

The known attitude matrix differential equation againWhereinFor the b system angular velocity of rotation relative to n system, substituted in (10):

$C_b^n\·{\mathrm{\ω}}_{nb}^b\×v^b+C_b^n\·{\stackrel{\·}{v}}^b+({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{in}^n)\×v^n-f^n=g^n---\left(11\right)$

In formula,Ignoring, above formula is further rewritten as formula (12)；

${\stackrel{\·}{v}}^b+{\mathrm{\ω}}_{ib}^b\×v^b-f^b=g^b---\left(12\right)$

Wherein, g^bFor terrestrial gravitation acceleration component in b system；F^bSpecific force sampled data for inertial measurement component；V^bBy speedometer speed sampling dataThe tachometric survey vector constitutedReplace；AndWith speedometer tachometric survey vector differentialReplace；It should be noted that owing to containing noise in speedometer speed sampling data, direct differentiation can amplify noise and cause that alignment precision declines, it is therefore desirable to use Nonlinear Tracking Differentiator pairCarry out differential and process that to obtain noise jamming less

In order to obtain matrixRequire over g^bWith gⁿBetween important relationship formula (13)；

$C_b^{i_{b0}}\·g^b=C_{i_0}^{i_{b0}}\·C_n^{i_0}\·g^n---\left(13\right)$

In formula, terrestrial gravitation acceleration component g under b systemⁿ=[00-g]^TFor known quantity；MatrixAll can be obtained by calculating above, therefore matrixSolve and only need to construct three not vectors in the same plane according to formula (13)；

In order to construct vector, attenuating noise simultaneously, it is integrated (13) formula both sides processing, obtains:

$z_t^{i_{b0}}=C_{i_0}^{i_{b0}}\·r_t^{i_0}---\left(14\right)$

In formula,

Obtain transition matrixRequire over g^bWith gⁿBetween important relationship formula

$C_b^{i_{b0}}\·g^b=C_{i_0}^{i_{b0}}\·C_n^{i_0}\·g^n---\left(15\right)$

Wherein,All can be obtained by calculating above；Above formula both sides integration is obtained

$z_t^{i_{b0}}=C_{i_0}^{i_{b0}}\·r_t^{i_0}---\left(16\right)$

Take two moment t₁And t₂Integrated valueWithThenCalculated by formula (17) and obtain；

$C_{i_0}^{i_{b0}}=\left[\begin{array}{c}{\left(z_{t_1}^{i_{b0}}\right)}^T\\ {\left(z_{t_1}^{i_{b0}}\×z_{t_2}^{i_{b0}}\right)}^T\\ {\left(z_{t_1}^{i_{b0}}\×z_{t_2}^{i_{b0}}\×z_{t_1}^{i_{b0}}\right)}^T\end{array}\right]\·{\left[\begin{array}{c}{\left(r_{t_1}^{i_0}\right)}^T\\ {\left(r_{t_1}^{i_0}\×r_{t_2}^{i_0}\right)}^T\\ {\left(r_{t_1}^{i_0}\×r_{t_2}^{i_0}\×r_{t_1}^{i_0}\right)}^T\end{array}\right]}^{-1}---\left(17\right)$

So far, in carrier movement process, utilize formula (6), (8) and (17) to calculate and obtain initially alignment finish time t_endRough attitude matrixBut this attitude matrix precision is not high, and now can not obtain carrier positions, it is therefore desirable to preserve sensor sample data to be further processed；

Step 3: Nonlinear Tracking Differentiator processes speedometer velocity differentials；

OrderThen there is following Nonlinear Tracking Differentiator

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=-r\·sign(x_1-v(t)+\frac{x_2\left|x_2\right|}{2r})\end{array}\right.---\left(18\right)$

In formula, r is Nonlinear Tracking Differentiator parameter, and v (t) is containing noisy signal to be tracked, in this case speedometer speed sampling data；Assume that η is the sampling period, then discretization computational methods are as follows；

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+\mathrm{\η}\·x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)-\mathrm{\η}\·r\·sign\left(x_1\right(k)-v(k+1)+\frac{x_2\left(k\right)\left|x_2\left(k\right)\right|}{2r})\end{array}\right.---\left(19\right)$

So far, obtain being disturbed the speedometer velocity differentials that sound pollution degree is less；

Step 4: utilize the sensor subsystem sampled data of storage, reverse Attitude Tracking calculates initial alignment start time attitude matrix；

Reverse Attitude Tracking algorithm is the attitude matrix in order to be obtained by coarse alignmentReversely calculate back and be initially directed at start time, obtain attitude matrixTo determine for fine alignment below and position and to lay the first stone；

Represent attitude of carrier with quaternary number q, then the resolving of attitude is obtained by quaternion differential equation (20)；

$\stackrel{\·}{q}=-\frac{1}{2}\·{\mathrm{\Ω}}_{nb}^b\·q---\left(20\right)$

Wherein,

${\mathrm{\Ω}}_{nb}^b=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]---\left(21\right)$

${\mathrm{\ω}}_{nb}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]={\mathrm{\ω}}_{ib}^b-C_n^b\·({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)---\left(22\right)$

In formula,For b system relative to component in b system of the angular velocity of n system；For rotational-angular velocity of the earth component in n system；For n system relative to component in n system of the angular velocity of terrestrial coordinate system and e system；

In the forward of attitude resolves, it it is the gyroscope sampled data of attitude quaternion q (k) and this moment k+1 utilizing a upper moment kRemove to solve the attitude quaternion q (k+1) in k+1 moment；This calculating process adopts and finishes card solving method；Finishing card solving method is the common method solving attitude quaternion with angle increment, and its three rank approximate calculation formula is:

$q(k+1)=\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{48})\·{\mathrm{\Ω}}_{nb}^b(k+1)\·T_s\]\·q\left(k\right)---\left(23\right)$

In formula, T_sFor the posture renewal cycle；

$\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}(k+1)={\mathrm{\ω}}_{nb}^b(k+1)\·T_s\\ ={\mathrm{\ω}}_{ib}^b\left(k+1\right)\·T_s-C_n^b\left(k\right)\·\[{\mathrm{\ω}}_{ie}^n\left(k\right)+{\mathrm{\ω}}_{en}^n\left(k\right)\]\·T_s\end{array}---\left(24\right)$

Δ θ (k+1)=| | Δ θ (k+1) | | (25)

And in reverse Attitude Tracking calculates, be attitude quaternion q (k+1) and the inertial measurement component angular velocity sampled data in k moment in known k+1 momentRemove to solve attitude quaternion q (k) in k moment；Therefore, formula (23) is rewritten as formula (26)；

$\begin{array}{c}q\left(k\right)={\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{8})+\left(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{(k+1)}^2}{48}\right)\·{\mathrm{\Ω}}_{nb}^b(k+1)\·T_s\]}^{-1}\·q(k+1)\\ \≈{\[I(1-\frac{\mathrm{\Δ}\mathrm{\θ}{\left(k\right)}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\Δ}\mathrm{\θ}{\left(k\right)}^2}{48})\·{\mathrm{\Ω}}_{nb}^b\left(k\right)\·T_s\]}^{-1}\·q\left(k+1\right)\end{array}---\left(26\right)$

Wherein,

$\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\left(k\right)={\mathrm{\ω}}_{nb}^b\left(k\right)\·T_s\\ ={\mathrm{\ω}}_{ib}^b\left(k\right)\·T_s-C_n^b\left(k+1\right)\·\[{\mathrm{\ω}}_{ie}^n(k+1)+{\mathrm{\ω}}_{en}^n(k+1)\]\·T_s\end{array}---\left(27\right)$

After considering coarse alignment, the positional information of carrier is unknowable, thereforeApproximate value isSimultaneously in order to improve the computational accuracy of Δ θ (k), speedometer speed sampling data are used to calculateIt is as follows that it calculates process:

${\mathrm{\ω}}_{en}^n(k+1)={\left[\begin{array}{ccc}-\frac{v_{DN}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)\·\tan L_0}{R_e}\end{array}\right]}^T---\left(28\right)$

Wherein,

So far, through type (26), (27) and (28) is by attitude matrixFollow the tracks of back and be initially directed at start time, obtainThus laying a solid foundation for ensuing fine alignment；

Step 5: utilize the sensor subsystem sampled data of storage to carry out inertial navigation resolving, set up Kalman filter and carry out fine alignment, obtain accurate attitude matrix and carrier positions information；

A. inertial navigation resolves and speedometer dead reckoning

To be initially directed at the latitude L of start time₀, longitude λ₀, height h₀And reverse Attitude Tracking resultFor resolving initial value, using the sensor sample data of storage to carry out attitude, speed and position and resolve, the inertial navigation obtaining each sample point resolves position p=[L λ h]^T, inertial navigation resolve speedInertial navigation resolves attitude matrixWith speedometer DR position p_D=[L_Dλ_Dh_D]^T, calculate speed

B. filter state model is built

Using n system as the reference frame of inertial navigation, filter state model is as follows:

In formula,For Inertial Navigation Platform misalignment；δ vⁿFor inertial navigation velocity error；δ p is inertial navigation site error；ε is the drift of Inertial Navigating Gyro；For inertial navigation accelerometer zero is inclined；δ p_DFor speedometer DR position error；δ K_DScale factor error for speedometer；Known terrestrial equator radius R_e, matrix M₁To M₆It is respectively as follows:

$M_1=\left[\begin{array}{ccc}0& -\frac{1}{R_e}& 0\\ \frac{1}{R_e}& 0& 0\\ \frac{\tan L}{R_e}& 0& 0\end{array}\right]---\left(30\right)$

$M^{\′}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{ie}\sin L& 0& 0\\ {\mathrm{\ω}}_{ie}\cos L& 0& 0\end{array}\right];M^{\′\′}=\left[\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ 0& 0& -\frac{v_E^n}{{R_e}^2}\\ \frac{v_E^n{\sec }^{2}L}{R_e}& 0& -\frac{v_E^n\tan L}{{R_e}^2}\end{array}\right]---\left(31\right)$

M₂=M '+M " (32)

$M_3=(v^n\×)\·M_1-\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×)---\left(33\right)$

M₄=(vⁿ×)·(2M′+M″)(34)

$M_5=\left[\begin{array}{ccc}0& \frac{1}{R_e}& 0\\ \frac{\sec L}{R_e}& 0& 0\\ 0& 0& 1\end{array}\right]---\left(35\right)$

$M_6=\left[\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ \frac{v_E^n\tan L\sec L}{R_e}& 0& -\frac{v_E^{n}secL}{{R_e}^2}\\ 0& 0& 0\end{array}\right]---\left(36\right)$

Filter state model is abbreviated as

$\stackrel{\·}{X}=F\·X+G\·W---\left(37\right)$

Wherein, systematic state transfer matrix F ∈ R^19×19, its matrix is specifically as shown in formula (38)；G is system noise transfer battle array；W is system noise；

$F=\left[\begin{array}{ccccccc}-{\mathrm{\ω}}_{in}^n\×& M_1& M_2& -C_b^n& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ f^n\×& M_3& M_4& 0_{3\×3}& C_b^n& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& M_5& M_6& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ M_5\·\left(v_D^n\×\right)& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& M_6& v_D^n\\ 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×1}\end{array}\right]---\left(38\right)$

C. measurement model is built

Inertial navigation is resolved position p and speedometer DR position p_DDifference as measuring vector Z, speedometer is measured the DR position error that noise causes and is established as the measurement noise of measurement model；

Z=p-p_D=δ p-δ p_D+ V=H X+V (39)

In formula, H is for measuring transfer matrix, H=[0_3×6I_3×30_3×6-I_3×30_3×1]；V is position measurement noise；Measure vector Z=[L-L_Dλ-λ_Dh-h_D]^T；

The inertial navigation attitude matrix that the inertial navigation error correction finally estimated with wave filter resolvesObtain accurate inertial navigation attitude matrixMeanwhile, the Accurate Estimation position p of speedometer is also obtained_D；Achieve the accurate attitude tracking of inertial navigation and position navigation；

Pass through above step, the autonomous type inertial navigation based on reverse Attitude Tracking proposed by the invention advance between Initial Alignment Method, take full advantage of sensor sample data, when just knowing that carrier initial position, running accurate alignment attitude battle array can be realized obtain and position navigation, substantially increase carrier maneuverability, be Initial Alignment Method between a kind of effective traveling。

2. according to claim 1 a kind of based on Initial Alignment Method between the autonomous type inertial navigation traveling of reverse Attitude Tracking, it is characterised in that: " sensor subsystem " described in step 2, it includes inertial measurement component and speedometer；Inertial measurement component records carrier relative to inertial space angular velocity and specific force, obtains angular velocity and specific force sampled data；Bearer rate measured by speedometer, obtains bearer rate sampled data。