CN103344251A - Transfer-alignment time-delay estimation method based on matching of speed and specific force - Google Patents

Abstract

The invention discloses a transfer-alignment time-delay estimation algorithm based on matching of speed and specific force. The method is realized by: carrying out coarse alignment of a sub inertial navigation system by using an aligned main inertial navigation system, then taking the speed difference and the specific force difference of the sub inertial navigation system relative to the main inertial navigation system as filtering observed quantities, expanding the time delay amount of main inertial navigation information into a filtering system state variable, combining an error model of the sub inertial navigation system, using Kalman filtering algorithm to estimate the time delay, and to estimate a platform error angle of the sub inertial navigation relative to the main inertial navigation. The method is applicable in the conditions that the main inertial navigation and the sub inertial navigation are both platform type navigation systems, and the transfer alignment based on the matching of the speed and the specific force has a shorter time delay.

Description

A kind of Transfer Alignment time delay estimation method that adds the specific force coupling based on speed

Technical field

The present invention relates to the method for estimation of the time delay that in the method for estimation, particularly a kind of inertial navigation system Transfer Alignment that a kind of information time postpones main inertial navigation match information existed.

Background technology

Transfer Alignment is to mate with the output information that is installed in the sub-inertial navigation (misalignment) on the carrier by the output information to carrier master inertial navigation (aiming at) under dynamic condition, estimate the misalignment of sub-inertial navigation, thereby finish a kind of method of sub-inertial navigation initial alignment.For Platform Inertial Navigation System, because the realization of Transfer Alignment filtering algorithm is carried out in sub-inertial navigation system, this just need arrive sub-inertial navigation system with some related information transmission of main inertial navigation system, because the existence of time delay, navigation information when the navigation information that causes sub-inertial navigation to receive transmits with main inertial navigation has certain error, sub-inertial navigation is carried out Transfer Alignment with the information that receives as benchmark, will influence the estimated accuracy for the platform error angle.

Interim at " Chinese inertial technology journal " the 13rd volume the 1st that publish in February, 2005, document " measuring the compensation method that postpones in the Transfer Alignment " employing is estimated the method that time delay expands to state variable in real time, but only adding the attitude Transfer Alignment at speed analyzes, because time delay is bigger to the influence of attitude equal angular movement information, therefore employing speed adds than force information and carries out Kalman filtering, can obtain better time delay estimation effect.

Summary of the invention

The object of the present invention is to provide a kind ofly at less message delay time error, estimating speed is faster, precision is higher adds the Transfer Alignment time delay estimation method of specific force coupling based on speed.

The objective of the invention is to realize by following steps:

(1) sub-inertial navigation preheating, and utilize the platform-type main inertial navigation system antithetical phrase inertial navigation system of having aimed to carry out a step Transfer Alignment, finish the coarse alignment of sub-inertial navigation system;

(2) navigation calculation is carried out in main, sub-inertial navigation, and gathers main inertial navigation speed, transfers to sub-inertial reference calculation computing machine than force information;

(3) in sub-inertial navigation computer, structure master, sub-inertial navigation velocity contrast, specific force difference are carried out the iteration of normal scatter Kalman filtering and are resolved;

Wherein, main, sub-inertial navigation is Platform INS Inertial, and the sub-ins error model that adopts is:

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}^n=f^n\×{\mathrm{\φ}}^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×\mathrm{\δv}+{\▿}^n\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\φ}}}^n=-{\mathrm{\ω}}_{\mathrm{in}}^n\×{\mathrm{\φ}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{en}}^n+{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{\·}{\▿}}^n=0\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ϵ}}}^n=0\\ \mathrm{\Δ}\stackrel{\·}{t}=0\end{array}\right.$

Wherein, n is local horizontal coordinates, δ v ⁿBe the projection of sub-inertial navigation velocity error in n system; φ ⁿIt is the platform error angle between main inertial navigation and the sub-inertial navigation system; f ⁿBe the projection of sub-inertial navigation specific force output in n system; For the earth rotation angular speed is the projection of n in navigation,

For navigation is that the rotation angle speed of relative earth system is in the projection of n system;

Be the zero inclined to one side projection in n system of sub-inertial navigation accelerometer;

Be the relative ideal value

Error, and ε ^sBe sub-inertial navigation gyroscopic drift; Δ t represents time delay.

Selected system state variables is:

$X={\left[\begin{array}{ccccccccccc}\mathrm{\δ}{v}_x& \mathrm{\δ}{v}_y& {\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& \mathrm{\Δt}\end{array}\right]}^T$

System state equation is: $\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}$

Wherein

$A=\left[\begin{array}{ccccc}{A}_{11}& A_{12}& {\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="HDA00003323626600022.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;3}& 0_{2\&times;3}& 0\\ A_{21}& A_{22}& 0_{3\&times;3}& I_{3\&times;3}& 0\\ 0_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;3}& 0_{2\&times;3}& 0\\ 0_{4\&times;3}& 0_{4\&times;3}& 0_{4\&times;3}& 0_{4\&times;3}& 0\end{array}\right]$

$B=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="HDA00003323626600022.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;6}\\ 0_{3\&times;2}& I_{3\&times;3}& 0_{3\&times;6}\\ 0_{6\&times;2}& 0_{6\&times;3}& 0_{6\&times;6}\end{array}\right]$

In the formula

$A_{12}=\left[\begin{array}{ccc}0& {-f}_z& f_y\\ f_z& 0& {-f}_x\end{array}\right]$

$A_{22}=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{inu}}^n& {-\mathrm{\&omega;}}_{\mathrm{inn}}^n\\ {-\mathrm{\&omega;}}_{\mathrm{inu}}^{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA00003323626600022.png"\; state="\{\{state\}\}">n</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{ine}}^n\\ {\mathrm{\&omega;}}_{\mathrm{inn}}^n& {-\mathrm{\&omega;}}_{\mathrm{ine}}^{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA00003323626600022.png"\; state="\{\{state\}\}">n</figure-callout>}}& 0\end{array}\right].$

The system noise acoustic matrix is:

W＝[w _ax?w _ay?w _εx?w _εy?w _εz?0?0?0?0?0?0] ^T

Wherein,

Be latitude; R _eBe earth radius; f _x, f _y, f _zBe respectively f ⁿEast orientation, north orientation, day to component; w _Ax, w _AyBe accelerometer bias random white noise, w _{ε x}, w _{ε y}, w _{ε z}Be the gyroscopic drift random white noise.

The systematic perspective measurement is:

Z＝[δv _x?δv _y?δf _x?δf _y] ^T

Observation equation is:

Z=HX+V

$H\left[\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(1\right)\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(2\right)\\ 0& 0& 0& {-f}_z& f_y& 1& 0& 0& 0& 0& -\mathrm{Da}\left(1\right)\\ 0& 0& f_z& 0& {-f}_x& 0& 1& 0& 0& 0& -\mathrm{Da}\left(2\right)\end{array}\right]$

Wherein, $\mathrm{Dv}\left(t\right)={\stackrel{\&CenterDot;}{V}}_m^n\left(t\right),$ $\mathrm{Da}\left(t\right)={\stackrel{\&CenterDot;}{f}}_m^n\left(t\right),$ And have ${\stackrel{\&CenterDot;}{V}}_m^n(t-\mathrm{\&Delta;t})={\stackrel{\&CenterDot;}{V}}_m^n\left(t\right),$ ${\stackrel{\&CenterDot;}{f}}_m^n(t-\mathrm{\&Delta;t})={\stackrel{\&CenterDot;}{f}}_m^n\left(t\right).$

(4) after filtering finishes, obtain time delay estimated value and time delay equalization after the platform error angle.

This Transfer Alignment time delay estimation method has good time delay estimation effect, real-time follow-up time delay preferably.Step is simple, enforcement is convenient, is applicable to main inertial navigation transmission of Information delay error in the Transfer Alignment is compensated.Transmission of Information can promote main, sub-Inertial navigation platform error angle φ after time delay effectively between main, the sub-inertial navigation estimating to obtain ⁿEstimated accuracy, realize sub-inertial navigation Transfer Alignment fast and effectively.

Description of drawings

Fig. 1 is the process flow diagram of this kind Transfer Alignment time delay estimation method.It is motor-driven down that Fig. 2 is that hull is done serpentine, and adding the specific force matching scheme with speed carries out Transfer Alignment, considers before the time delay equalization and after the compensation, the platform error angle estimation curve that emulation obtains.Fig. 3 is the real-time estimation curve of this algorithm for time delay.

Embodiment:

A kind ofly add the Transfer Alignment time delay estimation method of specific force coupling based on speed, this method is realized by following steps:

(1) sub-inertial navigation preheating, and utilize the platform-type main inertial navigation system antithetical phrase inertial navigation system of having aimed to carry out a step Transfer Alignment, finish the coarse alignment of sub-inertial navigation system; Also comprised the initialization of all the other navigation informations such as antithetical phrase inertial navigation speed, position in this step, made sub-inertial navigation system can begin independently to resolve and export navigation information.

(2) navigation calculation is carried out in main, sub-inertial navigation, and gathers main inertial navigation speed, transfers to sub-inertial reference calculation computing machine than force information;

(3) in sub-inertial navigation computer, structure master, sub-inertial navigation velocity contrast, specific force difference are carried out the iteration of normal scatter Kalman filtering and are resolved;

This step mainly comprises 4 parts: the 1) foundation of ins error equation; 2) foundation of system state equation; 3) foundation of observation equation; 4) the Kalman Filtering for Discrete iteration is resolved.

If it is that n is geographic coordinate system that the platform of Platform INS Inertial requires the navigation coordinate of simulation, the actual platform coordinate of setting up is n '.Owing to the error of calculation, execute the influence of square error and information source error, n ' is that relative n system has deviation angle φ ⁿ

1) according to the mechanics layout of Platform Inertial Navigation System, sets up the error model of inertial navigation.Its error equation is:

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}^n=f^n\&times;{\mathrm{\&phi;}}^n-(2{\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;\mathrm{\&delta;v}+{\&dtri;}^n\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}^n=-{\mathrm{\&omega;}}_{\mathrm{in}}^n\&times;{\mathrm{\&phi;}}^n+\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{ie}}^n+\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n+{\mathrm{\&epsiv;}}^n\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{\&dtri;}}^n=0\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}^n=0\\ \mathrm{\&Delta;}\stackrel{\&CenterDot;}{t}=0\end{array}\right.$

Wherein, δ v ⁿBe the projection of sub-inertial navigation velocity error in n system; φ ⁿIt is the platform error angle between main inertial navigation and the sub-inertial navigation system; f ⁿBe the projection of sub-inertial navigation specific force output in n system;

For the earth rotation angular speed is the projection of n in navigation,

For navigation is that the rotation angle speed of relative earth system is in the projection of n system;

Be the zero inclined to one side projection in n system of sub-inertial navigation accelerometer;

Be the relative ideal value

Error, and

ε ^sBe sub-inertial navigation gyroscopic drift; Δ t represents time delay, usually the main inertial navigation message delay time is made as an arbitrary constant that changes between 0～0.1s.

2) set up the state equation of system.In conjunction with above-mentioned ins error equation, ignore day to channel speed, information, and will be extended for state variable time delay, obtain system state amount and be:

$X={\left[\begin{array}{ccccccccccc}\mathrm{\&delta;}{v}_x& \mathrm{\&delta;}{v}_y& {\mathrm{\&phi;}}_x& {\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_z& {\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& \mathrm{\&Delta;t}\end{array}\right]}^T$

Then system state equation is: $\stackrel{\&CenterDot;}{X}=\mathrm{AX}+\mathrm{BW}$

Wherein

$A=\left[\begin{array}{ccccc}{A}_{11}& A_{12}& {\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="HDA00003323626600022.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;3}& 0_{2\&times;3}& 0\\ A_{21}& A_{22}& 0_{3\&times;3}& I_{3\&times;3}& 0\\ 0_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;3}& 0_{2\&times;3}& 0\\ 0_{4\&times;3}& 0_{4\&times;3}& 0_{4\&times;3}& 0_{4\&times;3}& 0\end{array}\right]$

$B=\left[\begin{array}{ccc}{I}_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;6}\\ 0_{3\&times;2}& I_{3\&times;3}& 0_{3\&times;6}\\ 0_{6\&times;2}& 0_{6\&times;3}& 0_{6\&times;6}\end{array}\right]$

In the formula

$A_{12}=\left[\begin{array}{ccc}0& {-f}_z& f_y\\ f_z& 0& {-f}_x\end{array}\right]$

$A_{22}=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{inu}}^n& {-\mathrm{\&omega;}}_{\mathrm{inn}}^n\\ {-\mathrm{\&omega;}}_{\mathrm{inu}}^{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA00003323626600022.png"\; state="\{\{state\}\}">n</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{ine}}^n\\ {\mathrm{\&omega;}}_{\mathrm{inn}}^n& {-\mathrm{\&omega;}}_{\mathrm{ine}}^{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA00003323626600022.png"\; state="\{\{state\}\}">n</figure-callout>}}& 0\end{array}\right].$

The system noise acoustic matrix is:

W＝[w _ax?w _ay?w _εx?w _εy?w _εz?0?0?0?0?0?0] ^T

Wherein,

Be latitude; R _eBe earth radius; f _x, f _y, f _zBe respectively f ⁿEast orientation, north orientation, day to component; w _Ax, w _AyBe accelerometer bias random white noise, w _{ε x}, w _{ε y}, w _{ε z}Be the gyroscopic drift random white noise.

3) set up the systematic observation equation that speed adds the specific force matching process.According to the system state equation of setting up, selection speed adds specific force to be measured as systematic perspective, then has:

Z＝[δv _x?δv _y?δf _x?δf _y] ^T

Speed and the specific force of supposing the main inertial navigation that sub-inertial navigation system (SINS) obtains constantly at t are that main inertial navigation is in t-Δ t data constantly.Velocity contrast between boss's inertial navigation and specific force difference are expressed as follows:

$\mathrm{\&Delta;V}=V_s^n\left(t\right)-V_m^n\left(t\right)$

$=V_s^n\left(t\right)-(V_m^n(t-\mathrm{\&Delta;t})+{\stackrel{\&CenterDot;}{V}}_m^n(t-\mathrm{\&Delta;t})\&CenterDot;\mathrm{\&Delta;t}$

In like manner,

$\mathrm{\&Delta;f}=f_s^n\left(t\right)-f_m^n\left(t\right)$

$=f_s^n\left(t\right)-(f_m^n(t-\mathrm{\&Delta;t})+{\stackrel{\&CenterDot;}{f}}_m^n(t-\mathrm{\&Delta;t})\&CenterDot;\mathrm{\&Delta;t})$

Because time of choosing of Δ t is very short, be defaulted as in time delay Δ t the rate of change of speed

Rate of change with specific force

Approximate constant in the cycle of calculating, namely ${\stackrel{\&CenterDot;}{V}}_m^n(t-\mathrm{\&Delta;t})={\stackrel{\&CenterDot;}{V}}_m^n\left(t\right),$ ${\stackrel{\&CenterDot;}{f}}_m^n(t-\mathrm{\&Delta;t})={\stackrel{\&CenterDot;}{f}}_m^n\left(t\right).$

Order $\mathrm{Dv}\left(t\right)={\stackrel{\&CenterDot;}{V}}_m^n\left(t\right),$ $\mathrm{Da}\left(t\right)={\stackrel{\&CenterDot;}{f}}_m^n\left(t\right),$ Then have:

$\mathrm{\&Delta;V}+\mathrm{Dv}\left(t\right)=V_s^n\left(t\right)-V_m^n(t-\mathrm{\&Delta;t})$

$\mathrm{\&Delta;f}+\mathrm{Da}\left(t\right)=f_s^n\left(t\right)-f_m^n(t-\mathrm{\&Delta;t})$

The observation equation that the speed that then can get adds the specific force coupling is:

Z=HX+V

$H=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(1\right)\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(2\right)\\ 0& 0& 0& {-f}_z& f_y& 1& 0& 0& 0& 0& -\mathrm{Da}\left(1\right)\\ 0& 0& f_z& 0& {-\mathrm{<figure-callout\; id="0"\; label="f\; x"\; filenames="HDA00003323626600022.png"\; state="\{\{state\}\}">f</figure-callout>}}_x& 0& 1& 0& 0& 0& -\mathrm{Da}\left(2\right)\end{array}\right]$

4) according to state equation and the observation equation of system, in conjunction with the speed of main, sub-inertial navigation with carry out the Kalman Filtering for Discrete iteration than force information and resolve.

(4) after the Kalman filtering iterative computation finishes, obtain the estimated value of time delay and the platform error angle after the time delay equalization.

The time delay estimation method that adds specific force coupling based on speed provided by the invention has the following advantages:

The one, this method only need be set up the contact of time delay and observed quantity, will be extended for quantity of state time delay, and only influences observation equation, implements comparatively simply, and calculated amount is few; The 2nd, this method can real-time compensating time delay add the influence that the specific force matching algorithm causes for speed in filtering; The 3rd, add attitude matching time delay equalization method than existing speed, the speed that this method adopts adds the specific force match information and is the line movable information, be subjected to a little less than the effect of time delay, therefore can well adapt to dynamic environment, to the good effect that estimates at of the measurement of time delay and attitude misalignment, satisfied the naval vessel under the real navigation condition in the Transfer Alignment to the real-time estimation of main inertial navigation time delay and the requirement that compensates.

In order to further specify the beneficial effect of described method, estimation and evaluated error to Transfer Alignment under following starting condition have been carried out emulation, simulation result such as Fig. 2, shown in Figure 3, and it has been carried out analysis compared.

Starting condition:

1) suppose that main inertial navigation is error free, the lever arm effect error is fully compensated.

2) carrier initial position: 117 ° of longitudes, 39 ° in latitude.Carrier initial attitude angle (pitching, rolling, course) is respectively: 0 °, and 0 °, 45 °; Sub-Inertial navigation platform initial error angle is ψ _x=5 ', ψ _y=5 ', ψ _z=5 '.

3) sub-inertial navigation gyroscope constant value drift be 0.01 (°)/h, the random drift noise be 0.001 (°)/h; The normal value zero of accelerometer is 1 * 10 partially ^-4G, zero inclined to one side noise is 1 * 10 at random ^-5G.

4) the state estimation initial value is 0; Initial variance battle array P ₀Parameter arranges according to above-mentioned inertia device error, guarantees that the Kalman filters the optimality of estimation.

P ₀＝diag{(0.1m/s) ²?(0.1m/s) ²?(5′) ²(5′) ²(5′) ²?(1×10 ^-4g) ²(1×10 ^-4g) ²(1°×10 ^-3) ²?(1°×10 ^-3) ²?(1°×10 ^-3) ²?0.05}

5) to do serpentine motor-driven for carrier, at first with the speed linear uniform motion of 10m/s, the motor-driven turning rate of serpentine be 1 (°)/s, the motor-driven 100s that lasts.In process of simulation, for the propagation delay phenomenon of considering to exist in the real process, utilize and to produce function at random the data of main inertial navigation (mainly be here the speed of main inertial navigation and than force information) are added that a delay is equally distributed time delay between 0～0.1s, the average of time delay is 0.05s, pass to sub-inertial navigation then, mate with the data of sub-inertial navigation, thereby finish the process of Transfer Alignment.

Analyze relatively:

Fig. 2 provides boats and ships and adds specific force coupling Transfer Alignment algorithm to the estimation of error curve of platform error angle in the motor-driven speed of utilizing down of serpentine, the estimation of error curve of blue solid lines for time delay not being estimated, green dotted line is the estimation of error curve after with this method time delay being estimated.Contrast as can be known, when boats and ships are done " S " when motor-driven, time delay is very big to the influence that speed adds specific force coupling Transfer Alignment algorithm, can not estimate the platform error angle when motor-driven doing.And this method effectively compensating time delay add the influence of specific force Transfer Alignment algorithm for speed, satisfy high precision and the rapidity of Transfer Alignment algorithm.As can be seen from Figure 3, this method can be real-time estimate time delay, for compensating time delay is established solid foundation to the influence that speed adds specific force coupling Transfer Alignment algorithm, because be 0～0.1s time delay, in order clearly to see estimation curve, get the preceding 20s estimation curve of time delay.

Claims (1)

1. one kind adds the Transfer Alignment time delay algorithm for estimating of specific force coupling based on speed, it is characterized in that:

(1) sub-inertial navigation preheating, and utilize the platform-type main inertial navigation system antithetical phrase inertial navigation system of having aimed to carry out a step Transfer Alignment, finish the coarse alignment of sub-inertial navigation system;

(2) navigation calculation is carried out in main, sub-inertial navigation, and gathers main inertial navigation speed, transfers to sub-inertial reference calculation computing machine than force information;

(3) in sub-inertial navigation computer, structure Kalman filtering fundamental equation, and utilize master, sub-inertial navigation velocity contrast, specific force difference to carry out normal scatter Kalman filtering iteration and resolve;

Wherein, main, sub-inertial navigation is Platform INS Inertial, and selected filtering system state variable is:

$X={\left[\begin{array}{ccccccccccc}\mathrm{\&delta;}{v}_x& \mathrm{\&delta;}{v}_y& {\mathrm{\&phi;}}_x& {\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_z& {\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& \mathrm{\&Delta;t}\end{array}\right]}^T$

In the formula, δ v _x, δ v _yBe respectively sub-inertial navigation east orientation, north orientation velocity error; φ _x, φ _y, φ _zBe respectively sub-inertial navigation with respect to the platform error angle of main inertial navigation;

Be respectively accelerometer bias; ε _x, ε _y, ε _zBe respectively gyroscopic drift; Δ t is the time delay of main inertial navigation information;

The zygote inertial navigation system error differential equation is set up the filter state equation;

The systematic perspective measurement that speed adds the specific force matching method is:

Z＝[δv _x?δv _y?δf _x?δf _y] ^T

In the formula, δ f _x, δ f _yBe respectively the poor of master, sub-inertial navigation specific force;

The filtering observation equation is:

Z=HX+V

$H=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(1\right)\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(2\right)\\ 0& 0& 0& {-f}_z& f_y& 1& 0& 0& 0& 0& -\mathrm{Da}\left(1\right)\\ 0& 0& f_z& 0& {-f}_x& 0& 1& 0& 0& 0& -\mathrm{Da}\left(2\right)\end{array}\right]$

In the formula, $\mathrm{Dv}\left(t\right)={\stackrel{\&CenterDot;}{V}}_m^n\left(t\right),$ $\mathrm{Da}\left(t\right)={\stackrel{\&CenterDot;}{f}}_m^n\left(t\right);$

Wherein,

Be respectively the differential of main inertial navigation speed and specific force;

(4) after Kalman filtering finishes, obtain time delay estimated value and time delay equalization after the platform error angle.

Images