DUAL-FILTER-BASED TRANSFER ALIGNMENT METHOD UNDER DYNAMIC DEFORMATION

Abstract

A dual-filter-based transfer alignment method under dynamic deformation. A dynamic deformation angle generated under dynamic deformation and a coupling angle between dynamic deformation and body motion will reduce the accuracy of transfer alignment; and a transfer alignment filter is divided into two parts, the first part estimates a bending deformation angle and the coupling angle, and uses an attitude matching method, and the second part estimates a dynamic lever arm, and uses a “speed plus angular speed” matching method.

Claims

1. A dual-filter-based transfer alignment method under dynamic deformation, applied to an aircraft wing deformation measurement system, in which a main inertial navigation system is installed in a cabin and inertial navigation subsystems are installed on the wings, wherein the method comprises the following steps: generating attitude, speed and position information of the main inertial navigation system and outputs of a gyro and an accelerometer by using a trajectory generator, simulating flexural deformation angle {right arrow over (θ)} and flexural deformation angular speed {dot over ({right arrow over (θ)})} between the main inertial navigation system and the inertial navigation subsystems by using a second-order Markov, carrying out geometric analysis on the flexural deformation, and deriving an expression of coupling angle Δ{right arrow over (ϕ)} resulted from dynamic deformation of the carrier and movement of the carrier; using the flexural deformation angle, the flexural deformation angular speed and the coupling angle as state quantities, and establishing a model of filter 1 with an attitude matching method; establishing a dynamic lever arm model by using the flexural deformation angle and the coupling angle estimated in step (2), and deriving an expression of speed error and an expression of angular speed error; establishing a model of filter 2 by using the expression of speed error and expression of angular speed error derived in step (3) with a “speed+angular speed” matching method, estimating the initial attitude error of the inertial navigation subsystems, and using this error for initial attitude calibration of the inertial navigation subsystems, so as to accomplish a transfer alignment process.

2. The dual-filter-based transfer alignment method under dynamic deformation according to claim 1, wherein in step (1), the geometrical analysis on the flexural deformation is carried out and the expression of coupling angle Δ{right arrow over (ϕ)} resulted from the dynamic deformation of the carrier and movement of the carrier is derived as follows:
Δ{right arrow over (ϕ)}=M{right arrow over (ω)}.sub.θ, wherein, {right arrow over (ω)}.sub.θ={dot over ({right arrow over (θ)})}, and M is expressed as: $M=\left[\begin{array}{ccc}0& 0& -\frac{1}{\omega \text{?}}\\ -\frac{1}{\omega \text{?}}& 0& 0\\ 0& -\frac{1}{\omega \text{?}}& 0\end{array}\right],\text{?}\text{indicates text missing or illegible when filed}$ wherein, ω.sub.isx.sup.s, ω.sub.isy.sup.s′ and ω.sub.isz.sup.s′ represent the ideal angular speeds of the inertial navigation subsystems in east, north, and sky directions respectively.

3. The dual-filter-based transfer alignment method under dynamic deformation according to claim 2, wherein in step (2), the flexural deformation angle, the flexural deformation angular speed and the coupling angle are used as state quantities and the model of filter 1 is established with an attitude matching method as follows: the state quantities of the filter 1 are selected as follows:
x.sub.1=[δ{right arrow over (ϕ)} {right arrow over (ε)}.sup.y {right arrow over (ρ)}.sub.0 {right arrow over (θ)} {dot over ({right arrow over (θ)})} Δ{right arrow over (ϕ)}].sup.r, wherein, δ{right arrow over (ϕ)} represents attitude error, {right arrow over (ε)}.sup.y represents zero drift of gyro measurement in the subsystem, and {right arrow over (ρ)}.sub.0 represents initial installation angle error between the main system and the subsystem; the state equation of filter 1 is:
{dot over (x)}.sub.1=F.sub.1x.sub.1+G.sub.1w.sub.1, wherein, F.sub.1 represents the state transition matrix of filter 1, G.sub.1 represents the system noise distribution matrix of filter 1, w.sub.1 represents the system noise of filter 1, the state transition matrix F.sub.1 is expressed as: $F_1=\left[\begin{array}{cccccc}\left(-\omega \text{?}\times \right)& -C\text{?}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& B_1& B_2& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& F_{64}& F_{65}& 0_{3\times 3}\end{array}\right],\text{?}\text{indicates text missing or illegible when filed}$ wherein, {right arrow over (ω)}.sub.in.sup.n represents the rotation of the navigation system with respect to the inertial system, {right arrow over (ω)}.sub.in.sup.nx represents an antisymmetric matrix, C.sub.s′.sup.m represents a transformation matrix between the ideal coordinate system of the subsystem and the navigation coordinate system, and $B_1=\left[\begin{array}{ccc}0& -2{\beta }_y& 0\\ 0& 0& -2{\beta }_z\\ -2{\beta }_x& 0& 0\end{array}\right]$ $B_2=\left[\begin{array}{ccc}0& -{\beta }_y^2& 0\\ 0& 0& -{\beta }_z^2\\ -{\beta }_x^2& 0& 0\end{array}\right],$ β.sub.i(i=x, y, z) represent the coefficients of the second-order Markov model in east, north, and sky directions, P=MB.sub.2, F.sub.65=MB.sub.1; the system measurement equation is: $y_1=H_1{x}_1+{\mu }_1,$ wherein, y.sub.1 represents the difference between the true value of attitude and the estimated value from the filter, H.sub.1 represents the measurement matrix of the filter 1, and μ.sub.1 represents the measurement noise in the filter 1.

4. The dual-filter-based transfer alignment method under dynamic deformation according to claim 2, wherein the expression of speed error and the expression of angular speed error are derived in step (3) as follows: the expression of angular speed error is as follows: $\Delta \stackrel{.fwdarw.}{\omega }=\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\stackrel{.fwdarw.}{\phi }+\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\Delta \stackrel{.fwdarw.}{\varphi }+A\left(\stackrel{.fwdarw.}{\omega }\text{?}\right)\stackrel{.fwdarw.}{u}\text{?}-A\left(\stackrel{.fwdarw.}{\omega }\text{?}\right)\left[\left(\frac{\pi }{2}U-\Delta \stackrel{.fwdarw.}{\varphi }\right)\times \right]\stackrel{.fwdarw.}{u}\text{?},\text{?}\text{indicates text missing or illegible when filed}$ wherein, θim.sup.m represents the angular speed of the main system in the coordinate system of the main system, {right arrow over (φ)} represents the ideal error angle between the main system and the subsystem, A({right arrow over (ω)}.sub.0) represent an amplitude matrix, {right arrow over (u)}.sub.is.sup.s represents the identity matrix in the direction of {right arrow over (ω)}.sub.is.sup.s, and {right arrow over (ω)}is.sup.s represents the angular speed of the subsystem under the coordinate system of the subsystem, U=[1 1 1].sup.T; the expression of speed error is as follows: $\delta \stackrel{.}{\stackrel{.fwdarw.}{v}}=-\left(2\omega \text{?}+\omega \text{?}\right)\times \delta \stackrel{.fwdarw.}{v}+R_0{B}_0\theta +\left(2\left(\omega \text{?}\times \right)R_0+R_0{B}_1\right)\widetilde{\stackrel{.fwdarw.}{\theta }}+C\text{?}{\nabla }^i+R_0\Delta \widetilde{\stackrel{.fwdarw.}{\varphi }}+\left[\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)+\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\right]\delta \stackrel{.fwdarw.}{r}+\left(f\text{?}\times \right)\Delta \stackrel{.fwdarw.}{\varphi }+\left[\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)+\left(\stackrel{.}{\stackrel{.fwdarw.}{\omega }}\times \right)\right]{\stackrel{.fwdarw.}{r}}_{\theta },\text{?}\text{indicates text missing or illegible when filed}$ {right arrow over (ω)}.sub.in.sup.n wherein, represents the rotation of the navigation system caused by the rotation of the earth, {right arrow over (ω)}.sub.im.sup.n represents the rotation of the navigation system caused by the curvature of the surface of the earth as the subsystem moves on the surface of the earth, θ{right arrow over (ν)}={right arrow over (ν)}.sub.s.sup.n−{right arrow over (ν)}.sub.m.sup.n, {right arrow over (ν)}.sub.m.sup.n and {right arrow over (ν)}.sub.s.sup.n represent the speed vectors of the main system and the subsystem in the navigation coordinate system, {right arrow over (θ)} represents the flexural deformation angle between the main inertial navigation system and the inertial navigation subsystem, Δ{right arrow over (ϕ)} represents the coupling angle between the main inertial navigation system and the inertial navigation subsystem, {right arrow over (ω)}.sub.im.sup.n represents the angular speed of the main system in the navigation system, C.sub.r.sup.m represents the transformation matrix between the subsystem and the navigation coordinate system, {right arrow over (∇)}.sup.s′ represents the zero drift of accelerometer measurement of the subsystem, f.sub.t.sup.N represents the specific force of the subsystem in the navigation coordinate system, β{right arrow over (r)} represents dynamic lever arm, {right arrow over (r)}.sub.0=[x.sub.0 y.sub.0 z.sub.0].sup.r represents static lever arm, x.sub.0 y.sub.0 z.sub.0 represent static lever arms in east, north, and sky directions respectively, and R.sub.0 can be expressed as: $R_0=\left[\begin{array}{ccc}0& z_0& 0\\ 0& 0& x_0\\ y_0& 0& 0\end{array}\right].$

5. The dual-filter-based transfer alignment method under dynamic deformation according to claim 4, wherein in step (4), the model of filter 2 is established with a “speed+angular speed” matching method, the equation of measurement quantities is established by using the expression of speed error and the expression of angular speed error derived in step (3), and a Kalman filter model is established as follows: the state quantities of the Kalman filter 2 are selected as follows:
x.sub.2=[β{right arrow over (ν)} {right arrow over (∇)}.sup.λ {right arrow over (θ)} {dot over ({right arrow over (θ)})} δ{right arrow over (r)}].sup.r, wherein, β{right arrow over (ν)} represents the speed error, and {right arrow over (∇)}.sup.λ represents the zero drift of accelerometer measurement of the subsystem; the state equation of the filter is: ${\stackrel{.}{x}}_2=F_2{x}_2+G_2{w}_2,$ wherein, G.sub.2 represents the system noise distribution matrix of filter 2, w.sub.2 represents the system noise of filter 2, and the state transition matrix F.sub.2 is expressed as: $F_2=\left[\begin{array}{ccccc}{F}_{11}& C_3^n& F_{13}& F_{14}& F_{15}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& B_1& B_2& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& F_{53}& F_{54}& 0_{3\times 3}\end{array}\right],$ wherein, F.sub.11=−[(2ω.sup.n+ω.sup.n)x], F.sub.13=R.sub.0B.sub.2+R.sub.0M(B.sub.1B.sub.2+B.sub.2), F.sub.14=(2(ω.sup.nx)R.sub.0+R.sub.0B.sub.1)+R.sub.0MB.sub.1.sup.2, F.sub.15=[({right arrow over (ω)}.sup.nx)({right arrow over (ω)}.sup.nx)+({dot over ({right arrow over (ω)})}.sup.nx)], F.sub.53=R.sub.0MB.sub.2, F.sub.54=R.sub.0+R.sub.0MB.sub.3, and the system measurement equation is as follows: $y_2=H_2{x}_2+{\mu }_2,$ wherein, y.sub.2 represents the difference between the true value of speed or angular speed and the estimated value from the filter, μ.sub.2 represents the measurement noise in the filter 2, and $H_2=\left[\begin{array}{ccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& \left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)& I_{3\times 3}& 0_{3\times 3}\end{array}\right].\text{?}\text{indicates text missing or illegible when filed}$

Description

BRIEF DESCRIPTION OF DRAWINGS

[0020] FIG. 1 is a flow chart of the transfer alignment based on dual filters according to the present invention;

[0021] FIG. 2 is a schematic diagram of the spatial relationship between angular speed vector and additional dynamic flexural angular speed vector;

[0022] FIG. 3 is a schematic diagram of the coupling angle (projected to the yoz plane) between the main inertial navigation system and the inertial navigation subsystems under dynamic deformation;

[0023] FIG. 4 is a schematic diagram of the relative position relationship between the main system and the subsystems.

DETAILED DESCRIPTION OF THE EMBODIMENTS

[0024] Hereunder the present invention will be further detailed in specific embodiments, with reference to the accompanying drawings.

[0025] As shown in FIG. 1, the transfer alignment method based on dual filters under dynamic deformation proposed in an embodiment of the present invention uses a trajectory simulator to simulate the attitude, speed and position of the main system of the aircraft and the output data of the inertia components, and uses a second-order Markov to simulate and output the flexural deformation angle {right arrow over (θ)} and flexural deformation angular speed {dot over ({right arrow over (θ)})} between the main system and the subsystems; it decouples the carrier movement and flexural deformation to obtain a coupling angle, and uses the coupling angle as a state quantity of filter 1, and uses an attitude matching method; the filter 2 utilizes the result of the filter 1 to compensate the lever arm error, and uses a speed+angular speed matching method. Hereunder detailed analysis is provided:

[0026] Step 1: generating attitude, speed and position information of the main inertial navigation system and outputs of inertia components (gyros and accelerometers) by using a trajectory generator, simulating flexural deformation angle {right arrow over (θ)} and flexural deformation angular speed {dot over ({right arrow over (θ)})} between the main inertial navigation system and the inertial navigation subsystems by using a second-order Markov, carrying out geometric analysis on the flexural deformation, and deriving the coupling angle Δ{right arrow over (ϕ)} between the main system and the subsystems resulted from dynamic deformation of the main system and the subsystems, wherein the flexural deformation angle {right arrow over (θ)} between the main system and the subsystems may be expressed by a second-order Markov as follows:

{umlaut over ({right arrow over (θ)})}=−2β{dot over ({right arrow over (θ)})}−β.sup.2{right arrow over (θ)}+ωr,

wherein, β=2.146/τ, τ represents correlation time, ω represents Gaussian white noise, and the ideal error angle vector {right arrow over (φ)} between the main system and the subsystem is expressed as:

{right arrow over (ω)}.sub.ik.sup.k′=C.sub.m.sup.s′({right arrow over (φ)}){right arrow over (ω)}.sub.im.sup.m

{right arrow over (φ)}={right arrow over (ϕ)}.sub.0+{right arrow over (θ)},

wherein, {right arrow over (ω)}.sub.ix.sup.s′ represents the gyro output of the subsystem in ideal state, {right arrow over (ω)}.sub.im.sup.m represents the gyro output of the main system, C.sub.m.sup.s′({right arrow over (φ)}): represents the attitude matrix between the main system and the subsystem, {right arrow over (ϕ)}.sub.0 represents the initial installation error angle vector of the main system, {right arrow over (θ)} represents the flexural deformation angle, additional angular speed {right arrow over (ω)}.sub.θ is generated under dynamic flexural deformation, and may be expressed as {right arrow over (ω)}.sub.ij={dot over ({right arrow over (θ)})}; then, as shown in FIG. 2, the angular speed output {right arrow over (ω)}.sub.is.sup.s of the subsystem in the actual state may be expressed as:

{right arrow over (ω)}.sub.is.sup.s=ω.sub.is.sup.s′+ω.sub.θ.

[0027] The coupling error angle vector Δ{right arrow over (ϕ)} between the main system and the subsystem caused by the flexural deformation coupling angular speed is:

Δ{right arrow over (ϕ)}=[Δϕ.sub.x Δϕ.sub.y Δϕ.sub.z].sup.r.

The subscript x, y, z represent east, north and up directions respectively, Δ{right arrow over (ϕ)} represents the coupling error angle between the main system and the subsystem caused by the flexural deformation coupling angular speed, i.e., the included angle between {right arrow over (ω)}.sub.ix.sup.s and {right arrow over (ω)}.sub.is.sup.s′, if ω.sub.is.sup.s′=[ω.sub.isx.sup.s′ ω.sub.isy.sup.s′ ω.sub.isz.sup.s′].sup.r, {circumflex over (ω)}.sub.g=[ω.sub.g.sub.x ω.sub.g.sub.y ω.sub.g.sub.z].sup.r, then:

{circumflex over (ω)}.sub.is.sup.s=[ω.sub.isx.sup.s′+ω.sub.θx ω.sub.isy.sup.s′+ω.sub.θy ω.sub.isz.sup.s′+ω.sub.θz].sup.r.

[0028] As shown in FIG. 3, based on the geometrical relationship:

[00011]$\hspace{1em}\{\begin{array}{c}{\mathrm{\Delta \varphi }}_x=\arctan \frac{\text{?}+\text{?}}{\text{?}+\text{?}}-\arctan \frac{\text{?}}{\text{?}}\\ {\mathrm{\Delta \varphi }}_y=\arctan \frac{\text{?}+\text{?}}{\text{?}+\text{?}}-\arctan \frac{\text{?}}{\text{?}}\\ {\mathrm{\Delta \varphi }}_z=\arctan \frac{\text{?}+\text{?}}{\text{?}+\text{?}}-\arctan \frac{\text{?}}{\text{?}}\end{array}\text{?}\text{indicates text missing or illegible when filed}$

The arctan function is expanded with Taylor series, and the high-order terms are omitted, then:

Δϕ=Mω.sub.θ=M{dot over (θ)},

wherein, M may be expressed as:

[00012]$M=\left[\begin{array}{ccc}0& 0& -\frac{1}{\text{?}}\\ -\frac{1}{\text{?}}& 0& 0\\ 0& -\frac{1}{\text{?}}& 0\end{array}\right];$$\text{?}\text{indicates text missing or illegible when filed}$

[0029] Step 2: using the flexural deformation angle, the flexural deformation angular speed and the coupling angle as state quantities, and establishing a model of filter 1 with an attitude matching method, as follows:

[0030] The state quantities of the filter 1 are selected as follows:

x.sub.1=[δ{right arrow over (ϕ)} {right arrow over (ε)}.sup.x {right arrow over (ρ)}.sub.0 {right arrow over (θ)} {dot over ({right arrow over (θ)})} Δ{right arrow over (ϕ)}].sup.r,

wherein, δ{right arrow over (ϕ)}represents attitude error, {right arrow over (ε)}.sup.s represents zero drift of gyro measurement in the subsystem, and {right arrow over (ρ)}.sub.0 represents initial installation angle error between the main system and the subsystem;

[0031] The state equation of filter 1 is:

{dot over (x)}.sub.1=F.sub.1x.sub.1+G.sub.1w.sub.1,

wherein, F.sub.1 represents a state transition matrix, G.sub.1 represents a system noise distribution matrix, w.sub.1 represents system noise, and, according to the coupling angle model obtained in step (1), the state transition matrix F.sub.1 may be expressed as:

[00013]$F_1=\left[\begin{array}{cccccc}\left(-\text{?}\times \right)& -\text{?}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& \text{?}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& B_1& B_2& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& F_{64}& F_{65}& 0_{3\times 3}\end{array}\right],\text{?}\text{indicates text missing or illegible when filed}$

wherein, {right arrow over (ω)}.sub.in.sup.μ represents the rotation of the navigation system with respect to the inertial system, {right arrow over (ω)}.sub.ix.sup.μx: represents an antisymmetric matrix, C.sub.s′.sup.n represents a transformation matrix between the ideal coordinate system of the subsystem and the navigation coordinate system, and

[00014]$B_1=\left[\begin{array}{ccc}0& -2\text{?}& 0\\ 0& 0& -2\text{?}\\ -2\text{?}& 0& 0\end{array}\right]B_2=\left[\begin{array}{ccc}0& -\text{?}& 0\\ 0& 0& -\text{?}\\ -\text{?}& 0& 0\end{array}\right],\text{?}\text{indicates text missing or illegible when filed}$

β.sub.i(i=x, y, z) represent the coefficients of the second-order Markov model in x, y, and z directions, F.sub.64=MB.sub.2, F.sub.65=MB.sub.1;

[0032] The system measurement equation is:

y.sub.1=H.sub.1x.sub.1+μ.sub.1,

wherein, y.sub.1 represents the difference between the true value of attitude and the estimated value from the filter; H.sub.1 represents the measurement matrix, see the document “Multi-mode Transfer Alignment based on Mechanics Modeling for Airborne DPOS, IEEE Sensors Journal, 2017, DOI: 10.1109/JSEN.2017.2771263” for the specific expression of the matrix H; μ.sub.1 represents the measurement noise in the filter 1;

[0033] Step 3: deriving an expression of speed error and an expression of angular speed error, as follows:

[0034] The angular speed difference Δ{right arrow over (ω)} between the train system and the subsystem may be expressed as:

Δ{right arrow over (ω)}={right arrow over (ω)}.sub.is.sup.x−{right arrow over (ω)}.sub.im.sup.m

Δ{right arrow over (ω)}={right arrow over (ω)}.sub.is.sup.x′+{right arrow over (ω)}.sub.θ−{right arrow over (ω)}.sub.im.sup.m,

wherein, the coupling error angle vector between the main system and the subsystem is Δ{right arrow over (ϕ)}, then the transformation matrix between the main system and the subsystem may be expressed as C.sub.m.sup.s′({right arrow over (φ)}+Δ{right arrow over (ϕ)}), and the error angular speed between the main system and the subsystem may be expressed as:

Δ{right arrow over (ω)}=C.sub.m.sup.s′({right arrow over (φ)}+Δ{right arrow over (ϕ)}){right arrow over (ω)}.sub.im.sup.m+{right arrow over (ω)}.sub.θ−{right arrow over (ω)}.sub.m.sup.m,

wherein, {right arrow over (ω)}.sub.θ′ is the projection of {right arrow over (ω)}.sub.is.sup.s′ on {right arrow over (ω)}.sub.is.sup.s; since the rotating vector ({right arrow over (φ)}+Δ{right arrow over (ϕ)}) from the main system to the subsystem is a small quantity, then:

[({right arrow over (φ)}+Δ{right arrow over (ϕ)})x]=I−C.sub.m.sup.x′({right arrow over (φ)}+Δ{right arrow over (ϕ)}).

[0035] Therefore:

Δ{right arrow over (ω)}={right arrow over (ω)}.sub.θ−[({right arrow over (φ)}+Δ{right arrow over (ϕ)})x]{right arrow over (ω)}.sub.im.sup.m,

wherein, [({right arrow over (φ)}+Δ{right arrow over (ϕ)})x] represents an antisymmetric matrix, and {right arrow over (ω)}.sub.θ′ is:

{right arrow over (ω)}.sub.θ′=A({right arrow over (ω)}.sub.θ)T({right arrow over (α)}){right arrow over (u)}.sub.is.sup.k,

wherein, A({right arrow over (ω)}.sub.θ) represents an amplitude matrix, {right arrow over (u)}.sub.is.sup.s represents the unit matrix M the direction of {right arrow over (ω)}.sub.is.sup.s, {right arrow over (α)} represents the included angle vector between {right arrow over (ω)}.sub.θ and {right arrow over (ω)}.sub.is.sup.k, T({right arrow over (α)}) represents the transformation matrix from {right arrow over (ω)}.sub.θ to

[00015]${\stackrel{.fwdarw.}{\omega }}_{\mathrm{ix}}^s,\stackrel{.fwdarw.}{\alpha }=\frac{\pi }{2}U-\Delta \stackrel{.fwdarw.}{\varphi },$

wherein U=[1 1 1].sup.r, and:

[00016]$A\left({\omega }_{\mathrm{ij}}\right)=\left[\begin{array}{ccc}.Math.{\omega }_{{\theta }_y}.Math.& 0& 0\\ 0& .Math.{\omega }_{{\theta }_y}.Math.& 0\\ 0& 0& .Math.{\omega }_{{\theta }_z}.Math.\end{array}\right]$${\stackrel{.fwdarw.}{u}}_{\mathrm{ix}}^R=\frac{\stackrel{.fwdarw.}{\omega }\text{?}}{.Math.\stackrel{.fwdarw.}{\omega }\text{?}.Math.}$$T\left(\stackrel{.fwdarw.}{\alpha }\right)=I-\left[\left(\frac{\pi }{2}U-\Delta \stackrel{.fwdarw.}{\varphi }\right)\times \right]$$\text{?}\text{indicates text missing or illegible when filed}$

The symbol ∥ represents modulus operation, {right arrow over (ω)}.sub.θ is substituted into the expression of Δ{right arrow over (ω)}, then:

Δ{right arrow over (ω)}=A({right arrow over (ω)}.sub.θ)T({right arrow over (α)}){right arrow over (u)}.sub.ix.sup.s−[({right arrow over (φ)}+Δ{right arrow over (ϕ)})x]{right arrow over (ω)}.sub.im.sup.m.

T({right arrow over (α)}) is substituted into the expression of Δ{right arrow over (ω)}, then:

[00017]$\Delta \stackrel{.fwdarw.}{\omega }=\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\stackrel{.fwdarw.}{\phi }+\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\Delta \stackrel{.fwdarw.}{\varphi }+A\left(\stackrel{.fwdarw.}{\omega }\text{?}\right)\stackrel{.fwdarw.}{u}\text{?}-A\left(\stackrel{.fwdarw.}{\omega }\text{?}\right)\left[\left(\frac{\pi }{2}U-\Delta \stackrel{.fwdarw.}{\varphi }\right)\times \right]\stackrel{.fwdarw.}{u}\text{?}.\text{?}\text{indicates text missing or illegible when filed}$

[0036] The positional relationship between the main system and the subsystem is shown in FIG. 4, and is expressed as:

[00018]${\stackrel{.fwdarw.}{R}}_s={\stackrel{.fwdarw.}{R}}_m+\stackrel{.fwdarw.}{r},$

wherein, {right arrow over (R)}.sub.m and {right arrow over (R)}.sub.y represent the vector of main node or subnode to the earth core respectively, {right arrow over (r)} represents the dynamic lever arm vector between the main node and the subnode, and the following expression is obtained in the inertial system:

[00019]$\stackrel{\text{?}}{R_{\text{?}}^{\text{?}}}\stackrel{\text{?}}{=R_{\text{?}}^{\text{?}}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right)\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{\text{?}}{r}}}^{\text{?}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{\text{?}}{r}}}^{\text{?}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{\text{?}}{r}}}^{\text{?}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{\text{?}}{r}}}^{\text{?}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{\text{?}}{r}}}^{\text{?}},\text{?}\text{indicates text missing or illegible when filed}$

wherein, C.sub.m.sup.i represents the transformation matrix from the main system to the inertial system, {right arrow over (r)}represents the dynamic lever arm vector in the coordinate system of the main system; according to Newton's second law:

[00020]${\stackrel{\stackrel{\text{?}}{.fwdarw.}}{R}}_s^i=f_s^i+g_s^i+{\stackrel{.fwdarw.}{\omega }}_{\text{?}}^i\times \left({\stackrel{.fwdarw.}{\omega }}_{\text{?}}^i\times {\stackrel{.fwdarw.}{R}}_{\text{?}}^i\right)$${\stackrel{\text{?}}{\stackrel{.fwdarw.}{R}}}_m^i=f_m^i+g_m^i+{\stackrel{.fwdarw.}{\omega }}_{\text{?}}^i\times \left({\stackrel{.fwdarw.}{\omega }}_{\text{?}}^i\times {\stackrel{.fwdarw.}{R}}_{\text{?}}^i\right),\text{?}\text{indicates text missing or illegible when filed}$

wherein, f.sub.s.sup.i represents the specific force of the subsystem in the inertial system, f.sub.m.sup.i represents the specific force of the main system in the inertial system, g represents gravitational acceleration, and {circumflex over (ω)}.sub.ie.sup.i represents the rotational angular speed of earth, then:

[00021]$\stackrel{\text{?}}{f_{\text{?}}^{\text{?}}}=f_{\text{?}}^{\text{?}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right)\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{\text{?}}{r}}}^{\text{?}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{\text{?}}{r}}}^{\text{?}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{.}{\stackrel{\text{?}}{r}}}}^{\text{?}}+C_{\text{?}}^{\text{?}}\left({\stackrel{\text{?}}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{\text{?}}{\stackrel{.}{\stackrel{\text{?}}{r}}}}^{\text{?}}+C_{\text{?}}^{\text{?}}{\stackrel{\text{?}}{\stackrel{\text{?}}{r}}}^{\text{?}}.\text{?}\text{indicates text missing or illegible when filed}$

Since the differential equations of speed vector of the main system and subsystem can be expressed as follows:

[00022]${\stackrel{.}{\stackrel{.fwdarw.}{v}}}_m^{\text{?}}=C_m^n{f}_m^m-\left(2{\stackrel{.fwdarw.}{\omega }}_{\text{?}}^n+{\stackrel{.fwdarw.}{\omega }}_{\text{?}}^n\right)\times {\stackrel{.fwdarw.}{v}}_{\text{?}}^n\times g^{\text{?}}$${\stackrel{.}{\stackrel{.fwdarw.}{v}}}_m^{\text{?}}=C_m^n{C}_s^m\left(f_s^s+{\stackrel{.fwdarw.}{\nabla }}^{\text{?}}\right)-\left(2{\stackrel{.fwdarw.}{\omega }}_{\text{?}}^{\text{?}}+{\stackrel{.fwdarw.}{\omega }}_{\text{?}}^{\text{?}}\right)\times {\stackrel{.fwdarw.}{v}}_{\text{?}}^n\times g^{\text{?}},\text{?}\text{indicates text missing or illegible when filed}$

wherein, {right arrow over (ν)}.sub.m.sup.n and {right arrow over (ν)}.sub.s.sup.n represent the speed vectors of the main system and the subsystem in the navigation coordinate system respectively, {right arrow over (ω)}.sub.ie.sup.n represents the rotation of the navigation system caused by the rotation of the earth, {right arrow over (ω)}.sub.im.sup.n represents the rotation of the navigation system caused by the curvature of the surface of the earth as the subsystem moves on the surface of the earth, {right arrow over (ω)}.sub.im.sup.n represents the angular speed of the main system in the navigation system, f.sub.s.sup.n′ represents the specific force of the subsystem in the navigation coordinate system, {right arrow over (∇)}.sup.r represents the zero bias of the accelerometer measurement of the subsystem, the speed error vector equation is expressed as:

[00023]$\delta \stackrel{.fwdarw.}{v}={\stackrel{.fwdarw.}{v}}_s^0-{\stackrel{.fwdarw.}{v}}_{\omega }^0.$

[0037] Through differential treatment of the two sides of the above equation, the following equation is obtained:

[00024]$\delta \stackrel{.}{\stackrel{.fwdarw.}{v}}=-\left(2{\stackrel{.fwdarw.}{\omega }}_{\text{?}}^{\text{?}}+{\stackrel{.fwdarw.}{\omega }}_{\text{?}}^{\text{?}}\right)\times \delta \stackrel{.fwdarw.}{v}+\left[\left({\stackrel{.fwdarw.}{\omega }}_{\text{?}}^{\text{?}}\times \right)\left({\stackrel{.fwdarw.}{\omega }}_{\text{?}}^{\text{?}}\times \right)+\left({\stackrel{.fwdarw.}{\omega }}_{\text{?}}^{\text{?}}\times \right)\right]{\stackrel{.fwdarw.}{r}}^{\text{?}}+2\left({\stackrel{.fwdarw.}{\omega }}_{\text{?}}^{\text{?}}\times \right){\stackrel{.}{\stackrel{.fwdarw.}{r}}}^{\text{?}}+{\stackrel{.}{\stackrel{.fwdarw.}{r}}}^{\text{?}}+C_{\text{?}}^{\text{?}}{\stackrel{.fwdarw.}{\nabla }}^{\text{?}}+\left(f_s^{\text{?}}\times \right)\Delta {\stackrel{.fwdarw.}{\varphi }}^{\text{?}}.\text{?}\text{indicates text missing or illegible when filed}$

The dynamic lever arm vector may be expressed as:

[00025]$\stackrel{.fwdarw.}{r}={\stackrel{.fwdarw.}{r}}_0+\delta \stackrel{.fwdarw.}{r}$$\delta \stackrel{.fwdarw.}{r}=R_0\left(\stackrel{.fwdarw.}{\phi }+\Delta \stackrel{.fwdarw.}{\varphi }\right),$

wherein, {right arrow over (r)}.sub.0=[x.sub.0 y.sub.0 z.sub.0].sup.r represents the static lever arm, x.sub.0 y.sub.0 z.sub.0 represent the static lever arm in east, north, and up directions respectively,

[00026]$R_0=\left[\begin{array}{ccc}0& z_0& 0\\ 0& 0& x_0\\ y_0& 0& 0\end{array}\right];$

through differential treatment of the two sides of the above equation, the following equations are obtained:

[00027]$\stackrel{.}{\stackrel{.fwdarw.}{r}}=\delta \stackrel{.}{\stackrel{.fwdarw.}{r}}=R_0\left(\stackrel{.}{\stackrel{.fwdarw.}{\theta }}+\Delta \stackrel{.}{\stackrel{.fwdarw.}{\varphi }}\right)$$\stackrel{\text{?}}{\stackrel{\text{?}}{r}}=R_0\left(\stackrel{\text{?}}{\stackrel{\text{?}}{\theta }}+\Delta \stackrel{\text{?}}{\stackrel{\text{?}}{\varphi }}\right)=R_0{B}_1\stackrel{.}{\stackrel{.fwdarw.}{\theta }}+R_0{B}_2\stackrel{.fwdarw.}{\theta }+R_0\Delta \stackrel{\text{?}}{\stackrel{\text{?}}{\varphi }}.\text{?}\text{indicates text missing or illegible when filed}$

The expression of the dynamic lever arm is substituted into the speed error vector expression, then:

[00028]$\delta \stackrel{.}{\stackrel{.fwdarw.}{v}}=-\left(2\omega \text{?}+\omega \text{?}\right)\times \delta \stackrel{.fwdarw.}{v}+R_0{B}_0\theta +\left(2\left(\omega \text{?}\times \right)R_0+R_0{B}_1\right)\widetilde{\stackrel{.fwdarw.}{\theta }}+C\text{?}{\nabla }^i+R_0\Delta \widetilde{\stackrel{.fwdarw.}{\varphi }}+\left[\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)+\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\right]\delta \stackrel{.fwdarw.}{r}+\left(f\text{?}\times \right)\Delta \stackrel{.fwdarw.}{\varphi }+\left[\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)\left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)+\left(\stackrel{.}{\stackrel{.fwdarw.}{\omega }}\times \right)\right]{\stackrel{.fwdarw.}{r}}_{\Omega }.\text{?}\text{indicates text missing or illegible when filed}$

[0038] Step 4: adopting “speed+angular speed” matching method for the transfer alignment filter 2, establishing a measurement quantity equation by using the expression of speed error and expression of angular speed error derived in step 3, establishing a Kalman filter model, estimating the initial attitude error of the subnodes, and using this error for initial attitude calibration of the subnodes, so as to accomplish a transfer alignment process.

In this step, the state quantities of the filter 2 are selected as follows:

[00029]$x_2={\left[\begin{array}{ccccc}\delta \stackrel{.fwdarw.}{v}& {\stackrel{.fwdarw.}{\nabla }}^{*}& \stackrel{.fwdarw.}{\theta }& \stackrel{.}{\stackrel{.fwdarw.}{\theta }}& \delta \stackrel{.fwdarw.}{r}\end{array}\right]}^T,$

wherein, δ{right arrow over (ν)} represents the speed error, and {right arrow over (∇)}.sup.s represents the zero bias of accelerometer measurement of the subsystem;

[0039] The state equation of the filter is:

{circumflex over (x)}.sub.2=F.sub.2x.sub.2+G.sub.2w.sub.2,

wherein, G.sub.2 represents the system noise distribution matrix of filter 2, w.sub.2 represents the system noise of filter 2, and F.sub.2 is expressed as:

[00030]$F_2=\left[\begin{array}{ccccc}{F}_{11}& C_3^n& F_{13}& F_{14}& F_{15}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& B_1& B_2& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& F_{53}& F_{54}& 0_{3\times 3}\end{array}\right],$

wherein, F.sub.12=−[(2ω.sub.ie.sup.n+ω.sub.en.sup.n)x], F.sub.13=R.sub.0B.sub.2+R.sub.0M(B.sub.1B.sub.2+B.sub.2), F.sub.14=(2(ω.sub.i.sup.nx)R.sub.0+R.sub.0B.sub.1)+R.sub.0MB.sub.1.sup.2, F.sub.15=[({right arrow over (ω)}.sub.im.sup.nx)({right arrow over (ω)}.sub.im.sup.αx)+({dot over ({right arrow over (ω)})}.sub.im.sup.nx)], F.sub.53=R.sub.0MB.sub.2, F.sub.54=R.sub.0MB.sub.1, and the system measurement equation is as follows:

y.sub.2=H.sub.2x.sub.2+μ.sub.2,

[0040] wherein, y.sub.2 represents the difference between the true value of speed or angular speed and the estimated value from the filter, μ.sub.2 represents the measurement noise in the filter 2, and

[00031]$H_2=\left[\begin{array}{ccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& \left(\stackrel{.fwdarw.}{\omega }\text{?}\times \right)& I_{3\times 3}& 0_{3\times 3}\end{array}\right].\text{?}\text{indicates text missing or illegible when filed}$