CN106840201A - A kind of three position Alignment Methods with twin shaft indexing mechanism inertial navigation - Google Patents

Abstract

The invention discloses a kind of three position Alignment Methods with twin shaft indexing mechanism inertial navigation, comprise the following steps：1) improved Kalman filter model is set up；2) Analysis on Observability；3) the estimation analysis of convergence speed of Observable combination；4) three position autoregistration；The present invention can make the zero inclined Fast Convergent of the equivalent north gyro drift of SINS and equivalent day to accelerometer by improved model；The position Alignment Method of band twin shaft indexing mechanism inertial navigation three of proposition, it is adaptable to quick autoregistration of the SINS under initial any attitude corner condition；The present invention can effectively shorten the alignment time, improve the autoregistration precision of the SINS with twin shaft indexing mechanism.

Description

Three-position self-alignment method of strapdown inertial navigation with double-shaft indexing mechanism

[ technical field ] A method for producing a semiconductor device

The invention belongs to the field of strapdown inertial navigation, and provides a three-position self-alignment method of strapdown inertial navigation with a double-shaft indexing mechanism.

[ background of the invention ]

Modern wars put higher and higher requirements on strategic tactical missiles, and inertial navigation systems to improve the response speed and the hit precision of missile weapon systems. The improvement of the precision of the inertial sensor has high cost, long period and great difficulty, and is influenced by the weakness of domestic basic industry and the banning of China weapons abroad, so that breakthrough progress is difficult to achieve in a short period. And the accuracy of the inertial navigation system can be effectively improved by improving the model and reasonably designing the algorithm.

Compared with a platform type inertial navigation system, the strapdown inertial navigation system has the advantages of fast response, low cost, complete information and high reliability, and meanwhile, the precision of the strapdown inertial navigation system is improved, so that the strapdown inertial navigation system is prevented from being repeatedly detached from a carrier to carry out parameter calibration, system decomposition and maintenance are carried out, and the use cost is saved. A double-shaft indexing mechanism is added in the strapdown inertial navigation system, so that the disassembly-free missile self-checking, calibration and maintenance of the strapdown inertial navigation system are realized, the operability and maneuverability of missile weapon equipment are improved, and the strapdown inertial navigation system has important use value and significance for the development of weapon equipment in China.

The strapdown inertial navigation system is self-aligned, and the output information of the inertial sensing element is real-time calculated by the navigation computer according to an alignment program, so that the mathematical platform is continuously transformed to a direction cosine array which can accurately describe an ideal carrier coordinate system to a geographic coordinate system. The traditional strapdown inertial navigation system is free of an indexing mechanism, self-alignment is unit self-alignment, the essence of the system is that the direction of gravity acceleration is taken as a reference for horizontal alignment, and the alignment precision of the system mainly depends on the precision of two accelerometers. The essence of the method for azimuth alignment is to use the north component direction of the earth angular rate as the reference of azimuth alignment, and the alignment accuracy is mainly determined by east gyro drift. The strapdown inertial navigation system with the single-axis indexing mechanism can identify the drift of the equivalent east gyroscope under the condition that the attitude is close to the horizontal, and further improves the self-alignment precision. For the strapdown inertial navigation system with the double-shaft indexing mechanism, due to the existence of the indexing mechanism, multi-position alignment can be carried out through rotating the indexing mechanism, and the self-alignment error model is improved to design a three-position quick self-alignment method, so that the self-alignment precision of the strapdown inertial navigation system with the double-shaft indexing mechanism can be effectively improved.

[ summary of the invention ]

The invention aims to overcome the defects of the existing single-position self-alignment and double-position self-alignment technologies, and provides a three-position self-alignment method of strapdown inertial navigation of a double-shaft indexing mechanism.

In order to achieve the purpose, the invention adopts the following technical scheme to realize the purpose:

a three-position self-alignment method of strapdown inertial navigation with a double-shaft indexing mechanism comprises the following steps:

1) performing observability analysis on a Kalman filtering model taking a speed error as a measurement quantity, improving the filtering model, converting a new observability state combination, and analyzing observability and convergence speed of the combination;

2) and at the initial position, firstly rotating the inner frame by-90 degrees around the indexing mechanism, then rotating the outer frame by-90 degrees around the indexing mechanism, and taking the rotating position as the first position of self-alignment of the strapdown inertial navigation system to acquire the data of the gyroscope and the accelerometer of the inertial navigation system. According to the recordToThe output of the gyro and the accelerometer at the moment is used for carrying out the rough alignment of the inertial system and the fine alignment of the improved model to finish the drift of the equivalent northbound gyroAnd zero position of acceleration in the sky directionMeasuring drift;

3) and rotating by 90 degrees around the outer frame to obtain a second position, and statically collecting the output of the gyroscope and the accelerometer at the second position. According to the recordToThe output of the gyro and the accelerometer at the moment is used for carrying out the rough alignment of the inertial system and the fine alignment of the improved model to finish the drift of the equivalent northbound gyroAnd zero position of acceleration in the sky directionThe drift is measured.

4) And rotating the inner frame by 90 degrees to obtain a third position, and statically collecting the output of the gyroscope and the accelerometer at the third position. According to the recordToThe output of the gyro and the accelerometer at the moment is used for carrying out the rough alignment of the inertial system and the fine alignment of the improved model to finish the drift of the equivalent northbound gyroAnd zero position of acceleration in the sky directionThe drift is measured.

5) From the sum of three position-attitude matricesAndthe result of the measurement of the drift is reversely solved to obtain_x、_yAnd_z. Attitude matrix sum using third position_x、_yAnd_zcomputingFrom the sum of three position-attitude matricesAndthe result of the measurement and the bleaching is reversely solved to obtain ▽_x、▽_yAnd ▽_zThe sum of attitude matrices using the third position ▽_x、▽_yAnd ▽_zComputingAnd

6) calculating the misalignment angle phi of the alignment end time according to the improved model_E、φ_NAnd phi_U. And carrying out primary correction to obtain a final fine alignment result.

Compared with the prior art, the invention has the following beneficial effects:

the method can lead the equivalent north gyro drift of the strapdown inertial navigation system and the zero-bias rapid convergence of the equivalent sky accelerometer through improving the model; the three-position self-alignment method for the strapdown inertial navigation with the double-shaft indexing mechanism is suitable for quick self-alignment of a strapdown inertial navigation system under the condition of an initial arbitrary attitude angle; the invention can effectively shorten the alignment time and improve the self-alignment precision of the strapdown inertial navigation system with the double-shaft indexing mechanism.

[ description of the drawings ]

FIG. 1 is a schematic diagram of coordinate system and two frame axes layout when zero position locking;

FIG. 2 is a schematic diagram of an initial position and an indexing process;

FIG. 3 is a first position equivalent northbound gyro drift estimation curve;

FIG. 4 is a second position equivalent northbound gyro drift estimation curve;

FIG. 5 is a third position equivalent northbound gyro drift estimation curve;

FIG. 6 is a comparison curve of pitch angle error for two methods;

FIG. 7 is a graph comparing roll angle errors for two methods;

FIG. 8 is a plot of course angle error versus two methods.

[ detailed description ] embodiments

The invention is described in further detail below with reference to the accompanying drawings:

referring to fig. 1-8, the three-position self-alignment method of strapdown inertial navigation with a dual-axis indexing mechanism of the present invention comprises the following steps:

step 1, establishing an improved Kalman filtering model

And taking a northeast coordinate system as a navigation coordinate system, wherein the navigation coordinate system is represented by n, the body coordinate system is represented by b, and the inertial coordinate system is represented by i. Omega_ieThe subscript in (a) indicates the rotation angular velocity of the terrestrial coordinate system with respect to the inertial coordinate system,^brepresenting the projection of the equivalent gyro drift in the b-system, ▽^bRepresents the projection of the zero offset ▽ of the equivalent accelerometer in system bⁿAttitude error vector phi, gyro constant drift under navigation coordinate systemⁿAnd accelerometer zero offset ▽ⁿThe form is as follows:

in the self-alignment process, the carrier does not move, and the velocity error equation of the strapdown inertial navigation system is as follows:

wherein,F_vφ＝(fⁿ×), "×" represents a cross product operation.

The attitude error equation of the strapdown inertial navigation system is as follows:

wherein,R_M、R_Nthe radius of the longitude and latitude ring and the radius of the unitary mortise ring are respectively, and the latitude and the height are respectively L, h.

Shifting of equivalent gyro^bAnd equivalent accelerometer zero offset ▽^bRegarding the random constant error, selecting the state variables as follows: x ═ Vⁿφ ▽^{n n}]^ΤWhile at the same time with a speed error VⁿAs an external measurement, the system model can be expressed as follows:

whereinH＝[I 0 0 0]I is a 3-order identity matrix and 0 is a 3-order zero matrix. V is a measurement noise caused by interference such as jitter, and can be approximated as a white noise sequence, and the variance is assumed to be R.

Step 2, observability analysis

According to the observability analysis correlation theory of the continuous time linear stationary system, the observability analysis is carried out on the system represented by the formula (3) at a single position, and an observability matrix Q is constructed as [ H ]^Τ(HF)^Τ… (HF¹¹)^Τ]^ΤLet Q₁＝[H^Τ(HF)^Τ… (HF⁴)^Τ]^Τ。

Wherein

Order toWherein N is₂₁＝-F_vv，N₃₁＝-F_φv，

It can be shown that A is present when L.noteq.0 ° or 90 °₂Conversely, the same results as in formula (6) can be obtained in a similar manner when L is 0 ° or 90 °.

And (3) performing row transformation on the left multiplication matrix N of the formula (4) to obtain:

from line 5N of formula (5)₅₂HF+N₅₄HF³+HF⁴Available as 0, HF⁴Can be prepared from HF and HF³Linear representation, hence HF^k(k 4,5 … 11) can be made of HF and HF³And (4) linear representation. Thus rank(Q)＝rank(Q₁)＝rank(NQ₁)。

From formula (5), rank (NQ)₁) As 9, it can therefore be concluded that: at any fixed position, the rank of the observable matrix for the system represented by equation (3) is 9, and the fully observable condition is not satisfied. However, there are observable combinations that are now analyzed.

Note the bookThen Y is equal to Q₁X, orderWherein

Wherein

The observable combination of states for the system represented by equation (3) is as follows:

when the observability analysis is finished, the analysis result shows that the observability of the accelerometer is directly observable except for the speed error state and the zero offset of the equivalent sky direction accelerometerThe rest states are combinations in various forms, wherein the horizontal attitude error angle, the equivalent east accelerometer and the equivalent north accelerometer are zero offset into an observable state combination, the course error angle, the equivalent east gyro drift and the equivalent east accelerometer are zero offset into an observable state combination, the equivalent north gyro drift and the equivalent north accelerometer are zero offset into an observable state combination, and the equivalent sky gyro drift and the equivalent north accelerometer are zero offset into an observable state combination. It can be seen that when the unit position alignment method is adopted, phi_EAlignment error ofφ_NAlignment error ofφ_UAlignment error ofIt can be seen that the state estimation accuracy except for the zero offset of the equivalent antenna direction accelerometer is limited by the accuracy of the corresponding inertial device.

Step 3, the estimated convergence rate analysis of the observable combination

The convergence rate of the observable combination represented by equation (8) can be analyzed by equation (7). From equation (7), the first 9 th order of MNY corresponds to the measurement of observable combinations, therefore, only the first 9 rows of the MNY matrix need to be analyzed as follows:

[MN]_1:9Y＝[MNQ₁]_1:9X (9)

in the formula, subscripts 1:9 denote the first 9 rows of the matrix.

As can be seen from the formula (10), the corresponding state quantities of the first 3 rows of the formula (9) can be directly obtained from Z, the observable state combinations corresponding to the 4 th to 6 th rows can be obtained from Z and Z first order derivatives, and the observable state combinations corresponding to the 7 th to 9 th rows are analyzed as follows

NY＝NQ₁X (11)

From the formula (12)

Namely, it isEquation two-side simultaneous left-multiplication matrixCan obtain the productTake the first two lines as

The first two rows of formula (15) are recorded as

The combination of observable states corresponding to rows 7-8 can be selected from Z,Andthe combination of observable states corresponding to row 9 can be found in the equation Z,And Z⁽³⁾Thus obtaining the product. The higher the order in which the quantity measurement order information is required, the longer the time it takes to estimate the corresponding state. The following conclusions can therefore be drawn: in addition to the combination of states other than speed error, (PX)_4:6Has the fastest convergence rate (PX)_7:8Second, then (PX)₉And slowest, so far, the convergence rate analysis is finished.

Step 4, three-position self-alignment method

From formula (8) if_E、▽_EAnd ▽_NCan be quickly and accurately identified, namely, the initial misalignment angle phi can be solved_E,φ_N,φ_U. Thus, the problem of self-alignment translates into how to quickly and accurately identify_E、▽_EAnd ▽_NDuring the actual alignment process, with ▽_E，▽_NIn contrast to the above-mentioned results,_Ethe influence of (a) is large. To reduce_EIn the two-position, it is proposed to perform alignment again by a forward rotation of 90 ° about the azimuth axis. Calculated value of east-direction equivalent gyro drift at the positionWhereinThe equivalent northbound gyro drift at the second position, however, two problems exist, firstly, if the carrier is not in the horizontal plane during initial alignment, namely a certain pitch angle or roll angle exists, for example, the missile launching vehicle is in a slope or a tire at one side is on a step, the equivalent northbound gyro drift at the second position is not equal to the equivalent easbound gyro drift at the first position, at this time, the equivalent easbound gyro drift at the first position is not measurable, and secondly, in the two-position drift measurement method, the equivalent northbound gyro drift ▽ can not be measured_EAnd ▽_NThe accuracy of the horizontal alignment cannot be further improved by performing the identification. Based on the two problems, the patent provides a three-position alignment method suitable for the strapdown inertial navigation system with the double-shaft indexing mechanism on the basis of improving an alignment error model.

As shown in FIG. 1, an inner frame of the two-axis strapdown inertial navigation system can rotate along a Z axis of the inertial navigation system, and an outer frame of the two-axis strapdown inertial navigation system rotates along a Y axis of the inertial navigation system. In the initial alignment, the inner frame is controlled to rotate-90 ° about the Z-axis of the IMU first, and then the outer frame is controlled to rotate-90 ° about the X-axis of the IMU as the first position of the initial alignment. The fine kalman filter alignment of the improved model is performed at this position according to equation (9).

Where θ, γ, and ψ are the carrier pitch, roll, and heading angle, respectively._x，_yAnd_zequivalent gyro drift in x, y, z directions, ▽, respectively_x，▽_yAnd ▽_zThe equivalent accelerometers in the x, y and z directions are zero offset respectively.

The outer frame is controlled to rotate 90 about the X-axis of the IMU as the second position of initial alignment. The fine kalman filter alignment of the improved model is performed at this position according to equation (9).

The outer frame is controlled to rotate 90 about the Z-axis of the IMU as the third position for initial alignment. The fine kalman filter alignment of the improved model is performed at this position according to equation (9).

Equivalent northbound gyro drift at all three locations_NAnd an equivalent zenith accelerometer zero offset ▽_UCarry out identification, i.e.And▽_x、▽_yand ▽_zCan be obtained. According to equation (24), it is obtained:

wherein

If the matrix A is full rank, then_x、_yAnd_zcan be uniquely determined by equation (28).

At this time, the process of the present invention,can be obtained from formula (29).

Wherein C ═ cos γ cos ψ -sin γ sin θ sin ψ -cos θ sin ψ sin γ cos ψ + cos γ sin θ sin ψ ].

If the matrix B is full, ▽_x、▽_yAnd ▽_zCan be uniquely determined by equation (30).

At this time, the process of the present invention,andcan be obtained from formula (31).

Wherein

Alignment at the third position, according to the estimatedAndcompensating the estimation result to calculate the misalignment angle phi at the end of the alignment_E、φ_NAnd phi_U。

Step 5, discussion of special cases

_x、_y、_z、▽_x、▽_yAnd ▽_zThe observability problem of (a) is equivalent to the solving problem of equations (26) and (27), and for this purpose, two theorems are introduced first.

Theorem 1: the coefficient matrix rank (a) n of the homogeneous system of linear equations, which has a unique solution. The coefficient matrix rank of the homogeneous system of linear equations (A) is less than n, and the system of equations has numerous solutions.

Theorem 2: a is an n × n matrix, then

a) The essential condition of rank (a) ═ n is | a | ≠ 0, which is called a as a full rank matrix;

b) an adequate condition for rank (a) < n is | a | ═ 0.

Equation (26) can solve for the gyro drift on the premise that the matrix a is full rank, and therefore, the initial attitude angle θ at which | a |, is made 0 is solved for₀、γ₀And psi₀The initial attitude angle is theta₀、γ₀And psi₀When the temperature of the water is higher than the set temperature,_x、_yand_znot fully observable, and in the same way ▽ can be found_x、▽_yAnd ▽_zAn unobservable attitude angle initial value.

In the practical application process of the missile weapon system, the initial attitude angle of the missile launching vehicle is limited by the practical application environment, the pitch angle and the roll angle are changed within a certain range, and according to the practical use condition, theta is more than or equal to-45 degrees and less than or equal to 45 degrees, gamma is more than or equal to-45 degrees and less than or equal to 45 degrees, and psi is more than-180 degrees and less than or equal to 180 degrees are selected. Solving | a | ═ 0 to obtain: when theta is₀＝0°，ψ₀0 ° (or ψ)₀180 °), where | a | ═ 0, rank (a) < 3. At this time_x、_yAnd_znot completely observable. When theta is₀＝0°，ψ₀When the angle is 0 DEG, the initial position is Y-direction north, the first position and the second position are X-direction south, the third position is Y-direction north, and the X-direction gyro and the Y-direction gyro drift_xAnd_ycan be obtained by measuring drift of an equivalent north gyro and drift of a Z gyro_zNot observable. When theta is₀＝0°，ψ₀When the angle is 180 degrees, the initial position is Y-direction guide, the first position and the second position are X-direction north-pointing, the third position is Y-direction guide, and the X-direction and Y-direction gyros drift_xAnd_ycan be obtained by measuring drift of an equivalent north gyro and drift of a Z gyro_zNot observable. At this time, the gyro drift is found by the analysis_xAnd_yit is possible to observe that,_znot observable, but only set to_z＝0。

When theta is₀Not equal to 0 DEG or psi₀| a | ≠ 0, and rank (a) ═ 3 when ≠ 0 ° and 180 °. At this time_x、_yAnd_zcan be uniquely determined by equation (28).

Solving | B | ═ 0 to obtain: when theta is₀0 ° and γ₀When 0 ° is established, | B | ═ 0, rank (B) < 3, at which time ▽_x、▽_yAnd ▽_zNot completely observable. When theta is₀＝0°，γ₀0 deg. with the initial position pointing in the Z direction, the first position pointing in the Y direction, the second position pointing in the Y direction, and the third position pointing in the Z direction, and with the Y and Z accelerometers having zero offset ▽_yAnd ▽_zAll can be obtained by drifting of an equivalent skyndrometer, and the zero offset ▽ of the X-direction accelerometer_xNot observable, at this point, the accelerometer has a zero offset ▽ as seen by the analysis_yAnd ▽_zCan observe, ▽_xNot observable, but set to ▽_x＝0。

When theta is₀Not equal to 0 DEG or gamma₀Not equal to 0 °, B not equal to 0 °, rank (B) 3, ▽_x、▽_yAnd ▽_zCan be uniquely determined by equation (30).

[ CHECKING ]

To verify the correctness of the analysis, simulation verification is carried out, the equivalent drift of the gyroscope is taken as [ 0.030.030.03 ] °/h, the equivalent zero offset of the accelerometer is taken as [ 505050 ] mug, the sampling frequency is 200Hz, and the initial attitude of the strapdown inertial navigation system is att _0 ═ 15 degrees, 10 degrees and 30 degrees ]. When the alignment is started, the table body of the strapdown inertial navigation system rotates 90 degrees around the Z axis of the inner frame at first, and then rotates 90 degrees around the X axis of the outer frame at second to serve as a first position for alignment, the rotation process is 10 seconds, and data are collected at the first position for 90 seconds. And then rotating by 90 degrees around the X axis of the outer frame to serve as a second position for alignment, rotating for 10s, and acquiring data for 90s at the second position. And then rotating by 90 degrees around the Z axis of the inner frame to serve as a third position for alignment, wherein the rotating process is 10s, data are collected at the third position for 90s, and the alignment time is 300s in total. The alignment is carried out by respectively adopting a unit Kalman filtering alignment method, a double-position Kalman filtering alignment method and a three-position self-alignment method provided by the patent, and fig. 3-5 are estimation curves of equivalent northbound gyro drift and equivalent accelerometer sky zero offset at three positions. Table 1 shows the results of the alignment test using different methods, and fig. 6 to 8 are graphs showing the attitude estimation using different methods. As can be seen from the figure, the heading angle convergence speed and the accuracy of the three-position self-alignment method provided by the patent are obviously superior to those of the other two methods.

TABLE 1 results of different alignment methods

According to the simulation result, the initial alignment precision of the strapdown inertial navigation system can be effectively improved within the same time, the initial alignment precision is 10% of the error of the double-position alignment method and is 5% of the error of the unit-position alignment method. The course alignment error of the strapdown inertial navigation system can be effectively reduced by using the method provided by the patent.

The above-mentioned contents are only for illustrating the technical idea of the present invention, and the protection scope of the present invention is not limited thereby, and any modification made on the basis of the technical idea of the present invention falls within the protection scope of the claims of the present invention.

Claims (2)

1. A three-position self-alignment method of strapdown inertial navigation with a double-shaft indexing mechanism is characterized by comprising the following steps:

step 1, establishing an improved Kalman filtering model:

taking a northeast coordinate system as a navigation coordinate system, wherein the navigation coordinate system is represented by n, the body coordinate system is represented by b, and the inertial coordinate system is represented by i; omega_ieThe subscript in (a) indicates the rotation angular velocity of the terrestrial coordinate system with respect to the inertial coordinate system,^brepresenting the projection of the equivalent gyro drift in the b-system, ▽^bDenotes the equivalent additionProjection of zero offset ▽ of speedometer in b system, speed error vector VⁿAttitude error vector phi, gyro constant drift under navigation coordinate systemⁿAnd accelerometer zero offset ▽ⁿThe form is as follows:

${\mathrm{\&delta;V}}^n=\left[\begin{array}{c}\mathrm{\&delta;}{V}_E\\ {\mathrm{\&delta;V}}_N\\ {\mathrm{\&delta;V}}_U\end{array}\right],\mathrm{\&phi;}=\left[\begin{array}{c}{\mathrm{\&phi;}}_E\\ {\mathrm{\&phi;}}_N\\ {\mathrm{\&phi;}}_U\end{array}\right],{\&dtri;}^n=\left[\begin{array}{c}{\&dtri;}_E\\ {\&dtri;}_N\\ {\&dtri;}_U\end{array}\right],{\mathrm{\&epsiv;}}^n=\left[\begin{array}{c}{\mathrm{\&epsiv;}}_E\\ {\mathrm{\&epsiv;}}_N\\ {\mathrm{\&epsiv;}}_U\end{array}\right]$

in the self-alignment process, the carrier does not move, and the velocity error equation of the strapdown inertial navigation system is as follows:

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}^n=F_{vv}\&CenterDot;{\mathrm{\&delta;V}}^n+F_{v\mathrm{\&phi;}}\&CenterDot;\mathrm{\&phi;}+{\&dtri;}^n---\left(1\right)$

wherein,F_vφ＝(fⁿ×), "×" represents a cross product operation;

the attitude error equation of the strapdown inertial navigation system is as follows:

$\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=F_{\mathrm{\&phi;}v}\&CenterDot;{\mathrm{\&delta;V}}^n+F_{\mathrm{\&phi;}\mathrm{\&phi;}}\&CenterDot;\mathrm{\&phi;}-{\mathrm{\&epsiv;}}^n---\left(2\right)$

wherein,R_M、R_Nthe radiuses of longitude and latitude circles and the radiuses of unitary and mortise circles are respectively, and the latitude and the height are respectively L, h;

shifting of equivalent gyro^bAnd equivalent accelerometer zero offset ▽^bRegarding the random constant error, selecting the state variables as follows: x ═ Vⁿφ ▽^{n n}]^ΤWhile at the same time with a speed error VⁿAs an external measurement, the system model is expressed as follows:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{X}=F\&CenterDot;X\\ Z=H\&CenterDot;X+V\end{array}\right.---\left(3\right)$

whereinH＝[I 0 0 0]I is a 3-order identity matrix and 0 is a 3-order zero matrix; v is measurement noise caused by interference such as shaking, can be approximately regarded as a white noise sequence, and the variance is assumed to be R;

step 2, observability analysis:

according to the observability analysis correlation theory of the continuous time linear stationary system, the observability analysis is carried out on the system represented by the formula (3) at a single position, and an observability matrix Q is constructed as [ H ]^Τ(HF)^Τ… (HF¹¹)^Τ]^ΤLet Q₁＝[H^Τ(HF)^Τ… (HF⁴)^Τ]^Τ；

$Q_1=\left[\begin{array}{c}H\\ HF\\ {\mathrm{HF}}^2\\ {\mathrm{HF}}^3\\ {\mathrm{HF}}^4\end{array}\right]=\left[\begin{array}{cccc}I& 0& 0& 0\\ F_{vv}& F_{v\mathrm{\&phi;}}& I& 0\\ A_1& A_2& F_{vv}& -F_{v\mathrm{\&phi;}}\\ B_1& B_2& A_1& -A_2\\ C_1& C_2& B_1& -B_2\end{array}\right]---\left(4\right)$

Wherein

Order toWherein N is₂₁＝-F_vv，N₃₁＝-F_φv，

And (3) performing row transformation on the left multiplication matrix N of the formula (4) to obtain:

${\mathrm{NQ}}_1=\left[\begin{array}{c}H\\ N_{21}H+HF\\ N_{31}H+N_{32}HF+N_{34}{\mathrm{HF}}^3\\ N_{42}HF+{\mathrm{HF}}^2+N_{44}{\mathrm{HF}}^3\\ N_{52}HF+N_{54}{\mathrm{HF}}^3+{\mathrm{HF}}^4\end{array}\right]=\left[\begin{array}{cccc}I& 0& 0& 0\\ 0& F_{v\mathrm{\&phi;}}& I& 0\\ 0& F_{\mathrm{\&phi;}\mathrm{\&phi;}}& 0& -I\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]---\left(5\right)$

by the formula N₅₂HF+N₅₄HF³+HF⁴Available as 0, HF⁴From HF and HF³Linear representation, hence HF^kBoth of HF and HF³Linear representation, where k is 4,5 … 11; thus rank (Q) ═ rank (Q)₁)＝rank(NQ₁)；

From formula (5), rank (NQ)₁) As 9, it was concluded that: at any fixed position, the rank of the system observable matrix represented by equation (3) is 9, and the completely observable condition is not met; however, there are observable combinations, which are now analyzed:

note the bookThen Y is equal to Q₁X, orderWherein

$K_{11}=\left[\begin{array}{ccc}0& 0& 1\\ 0& \frac{1}{g}& 0\\ -\frac{1}{g}& 0& 0\end{array}\right],K_{21}=\left[\begin{array}{ccc}-\frac{\tan L}{g}& 0& 0\\ 0& -\frac{{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L}{g}& 0\\ 0& \frac{{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}{g}& 0\end{array}\right],K_{22}=\left[\begin{array}{ccc}-\frac{1}{{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]$

${\mathrm{MNQ}}_1=\left[\begin{array}{c}P\\ 0_{6\&times;12}\end{array}\right]---\left(6\right)$

Wherein

$MNY={\mathrm{MNQ}}_{1}X=\left[\begin{array}{c}P\\ 0_{6\&times;12}\end{array}\right]X---\left(7\right)$

The observable combination of states for the system represented by equation (3) is as follows:

$PX=\left[\begin{array}{c}{{\mathrm{\&delta;V}}^n}_{3\&times;1}\\ {\&dtri;}_U\\ {\mathrm{\&phi;}}_E+\frac{1}{g}{\&dtri;}_N\\ {\mathrm{\&phi;}}_N-\frac{1}{g}{\&dtri;}_E\\ {\mathrm{\&phi;}}_U+\frac{1}{{\mathrm{\&omega;}}_{ie}\cos L}{\mathrm{\&epsiv;}}_E-\frac{\tan L}{g}{\&dtri;}_E\\ {\mathrm{\&epsiv;}}_N-\frac{{\mathrm{\&omega;}}_{ie}\sin L}{g}{\&dtri;}_N\\ {\mathrm{\&epsiv;}}_U+\frac{{\mathrm{\&omega;}}_{ie}\cos L}{g}{\&dtri;}_N\end{array}\right]---\left(8\right)$

and 3, analyzing the estimated convergence rate of the observable combination:

analyzing the convergence rate of the observable combination represented by formula (8) by formula (7); from equation (7), the first 9 th order of MNY corresponds to the measurement of observable combinations, therefore, only the first 9 rows of the MNY matrix need to be analyzed as follows:

[MN]_1:9Y＝[MNQ₁]_1:9X＝PX (9)

where the subscripts 1:9 represent the first 9 rows of the matrix;

$\begin{array}{c}{\&lsqb;MN\&rsqb;}_{1:9}={\left(\left[\begin{array}{ccccc}I& 0& 0& 0& 0\\ 0& K_{11}& 0& 0& 0\\ 0& K_{21}& K_{22}& 0& 0\\ 0& 0& 0& I& 0\\ 0& 0& 0& 0& I\end{array}\right]\&CenterDot;\left[\begin{array}{ccccc}I& 0& 0& 0& 0\\ N_{21}& I& 0& 0& 0\\ N_{31}& N_{32}& 0& N_{34}& 0\\ 0& N_{42}& I& N_{44}& 0\\ 0& N_{52}& 0& N_{54}& I\end{array}\right]\right)}_{1:9}=\\ \left[\begin{array}{ccccc}I& 0& 0& 0& 0\\ K_{11}{N}_{21}& K_{11}& 0& 0& 0\\ K_{21}{N}_{21}+K_{22}{N}_{31}& K_{21}+K_{22}{N}_{32}& 0& K_{22}{N}_{34}& 0\end{array}\right]\end{array}---\left(10\right)$

according to the formula (10), the state quantities corresponding to the first 3 rows of the formula (9) can be directly obtained from Z, the observable state combinations corresponding to the 4 th to 6 th rows can be obtained from Z and Z first order derivatives, and the observable state combinations corresponding to the 7 th to 9 th rows are analyzed as follows:

NY＝NQ₁X (11)

$\left[\begin{array}{c}Z\\ N_{21}Z+\stackrel{\&CenterDot;}{Z}\\ N_{31}Z+N_{32}\stackrel{\&CenterDot;}{Z}+N_{34}{Z}^{\left(3\right)}\\ N_{42}\stackrel{\&CenterDot;}{Z}+\stackrel{\&CenterDot;\&CenterDot;}{Z}+N_{44}{Z}^{\left(3\right)}\\ N_{52}\stackrel{\&CenterDot;}{Z}+N_{54}{Z}^{\left(3\right)}+Z^{\left(4\right)}\end{array}\right]=\left[\begin{array}{c}\mathrm{\&delta;}V\\ F_{\mathrm{\&phi;}v}\mathrm{\&phi;}+{\&dtri;}^n\\ F_{\mathrm{\&phi;}\mathrm{\&phi;}}\mathrm{\&phi;}+{\mathrm{\&epsiv;}}^n\\ 0\\ 0\end{array}\right]---\left(12\right)$

from formula (12):

$N_{42}\stackrel{\&CenterDot;}{Z}+\stackrel{\&CenterDot;\&CenterDot;}{Z}+N_{44}{Z}^{\left(3\right)}=0---\left(13\right)$

namely, it isEquation two-side simultaneous left-multiplication matrixTo obtainThe first two rows are taken as:

${\&lsqb;{\mathrm{UF}}_{v\mathrm{\&phi;}}{N}_{34}{Z}^{\left(3\right)}\&rsqb;}_{1:2}=\left[\begin{array}{ccc}\frac{-1}{{\mathrm{\&omega;}}_{ie}\cos L}& 0& 0\\ 0& -1& 0\end{array}\right]N_{34}{Z}^{\left(3\right)}={\&lsqb;K_{22}{N}_{34}{Z}^{\left(3\right)}\&rsqb;}_{1:2}={\&lsqb;U\left(-N_{42}\stackrel{\&CenterDot;}{Z}-\stackrel{\&CenterDot;\&CenterDot;}{Z}\right)\&rsqb;}_{1:2}---\left(14\right)$

${\&lsqb;MN\&rsqb;}_{7:9}Y=(K_{21}{N}_{21}+K_{22}{N}_{31})Z+(K_{21}+K_{22}{N}_{32})\stackrel{\&CenterDot;}{Z}+K_{22}{N}_{34}{Z}^{\left(3\right)}---\left(15\right)$

the first two rows of equation (15) are taken and are recorded as:

$\begin{array}{c}{\&lsqb;MN\&rsqb;}_{7:8}Y={\&lsqb;\left(K_{21}{N}_{21}+K_{22}{N}_{31}\right)Z\&rsqb;}_{1:2}+{\&lsqb;\left(K_{21}+K_{22}{N}_{32}\right)\stackrel{\&CenterDot;}{Z}\&rsqb;}_{1:2}+{\&lsqb;K_{22}{N}_{34}{Z}^{\left(3\right)}\&rsqb;}_{1:2}=\\ {\&lsqb;\left(K_{21}{N}_{21}+K_{22}{N}_{31}\right)Z\&rsqb;}_{1:2}+{\&lsqb;\left(K_{21}+K_{22}{N}_{32}\right)\stackrel{\&CenterDot;}{Z}\&rsqb;}_{1:2}+{\&lsqb;U\left(-N_{42}\stackrel{\&CenterDot;}{Z}-\stackrel{\&CenterDot;\&CenterDot;}{Z}\right)\&rsqb;}_{1:2}\end{array}---\left(16\right)$

the combination of observable states corresponding to the 7 th to 8 th rows is composed of Z,Andthe combination of observable states corresponding to row 9 is obtained from Z,And Z⁽³⁾Obtaining; the higher the order in which the quantity measurement order information is needed, the longer the time it takes to estimate the corresponding state, and therefore the following conclusions are drawn: in addition to the combination of states other than speed error, (PX)_4:6Has the fastest convergence rate (PX)_7:8Second, then (PX)₉The slowest;

step 4, self-aligning three positions:

is obtained from the formula (8) if_E、▽_EAnd ▽_NCan be quickly and accurately identified, namely, the initial misalignment angle phi can be solved_E,φ_N,φ_U(ii) a An inner frame of the double-shaft strapdown inertial navigation system can rotate along a Z axis of the inertial navigation system, and an outer frame rotates along a Y axis of the inertial navigation system; during initial alignment, firstly controlling the inner frame to rotate to 90 degrees around the Z axis of the IMU, and then controlling the outer frame to rotate to 90 degrees around the X axis of the IMU to serve as a first position of the initial alignment; performing Kalman filtering fine alignment of the improved model at the position according to the formula (9);

$C_{b1}^n=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ -\cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&theta;}& -\cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]---\left(17\right)$

${\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_E^I& {\mathrm{\&epsiv;}}_N^I& {\mathrm{\&epsiv;}}_U^I\end{array}\right]}^T=C_{b1}^n{\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T---\left(18\right)$

${\left[\begin{array}{ccc}{\&dtri;}_E^I& {\&dtri;}_N^I& {\&dtri;}_U^I\end{array}\right]}^T=C_{b1}^n{\left[\begin{array}{ccc}{\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z\end{array}\right]}^T---\left(19\right)$

wherein theta, gamma and psi are the pitch, roll and course angle of the carrier respectively;_x，_yand_zequivalent gyro drift in x, y, z directions, ▽, respectively_x，▽_yAnd ▽_zThe equivalent accelerometers in the directions of x, y and z are zero offset respectively;

controlling the outer frame to rotate 90 degrees around the X axis of the IMU as a second position of initial alignment; performing Kalman filtering fine alignment of the improved model at the position according to the formula (9);

$C_{b2}^{b1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right]$

$\begin{array}{c}{C}_{b2}^n=C_{b1}^n{C}_{b2}^{b1}=\\ \left[\begin{array}{ccc}\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ -\cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]\end{array}---\left(20\right)$

${\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_E^{II}& {\mathrm{\&epsiv;}}_N^{II}& {\mathrm{\&epsiv;}}_U^{II}\end{array}\right]}^T=C_{b2}^n{\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T---\left(21\right)$

${\left[\begin{array}{ccc}{\&dtri;}_E^{II}& {\&dtri;}_N^{II}& {\&dtri;}_U^{II}\end{array}\right]}^T=C_{b2}^n{\left[\begin{array}{ccc}{\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z\end{array}\right]}^T---\left(22\right)$

controlling the outer frame to rotate 90 degrees around the Z axis of the IMU, and taking the rotation as a third position of initial alignment; performing Kalman filtering fine alignment of the improved model at the position according to the formula (9);

$C_{b3}^{b2}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$

$\begin{array}{c}{C}_{b3}^n=C_{b1}^n{C}_{b2}^{b1}{C}_{b3}^{b2}=\\ \left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]\end{array}---\left(23\right)$

${\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_E^{III}& {\mathrm{\&epsiv;}}_N^{III}& {\mathrm{\&epsiv;}}_U^{III}\end{array}\right]}^T=C_{b3}^n{\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T---\left(24\right)$

${\left[\begin{array}{ccc}{\&dtri;}_E^{III}& {\&dtri;}_N^{III}& {\&dtri;}_U^{III}\end{array}\right]}^T=C_{b3}^n{\left[\begin{array}{ccc}{\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z\end{array}\right]}^T---\left(25\right)$

equivalent northbound gyro drift at three locations_NAnd an equivalent zenith accelerometer zero offset ▽_UCarry out identification, i.e.And▽_x、▽_yand ▽_zObtaining; according to equation (24), we obtain:

${\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_N^I& {\mathrm{\&epsiv;}}_N^{II}& {\mathrm{\&epsiv;}}_N^{III}\end{array}\right]}^T=A{\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T---\left(26\right)$

${\left[\begin{array}{ccc}{\&dtri;}_U^I& {\&dtri;}_U^{II}& {\&dtri;}_U^{III}\end{array}\right]}^T=B{\left[\begin{array}{ccc}{\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z\end{array}\right]}^T---\left(27\right)$

wherein

$A=\left[\begin{array}{ccc}-\cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ -\cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\end{array}\right]$

$B=\left[\begin{array}{ccc}-\sin \mathrm{\&theta;}& -\cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ -\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]$

If the matrix A is full rank, then_x、_yAnd_zuniquely determined by equation (28):

${\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T=A^{-1}{\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_N^I& {\mathrm{\&epsiv;}}_N^{II}& {\mathrm{\&epsiv;}}_N^{III}\end{array}\right]}^T---\left(28\right)$

at this time, the process of the present invention,obtained by the formula (29):

${\mathrm{\&epsiv;}}_E^{III}=C{\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T---\left(29\right)$

wherein C ═ C [ cos γ cos ψ -sin γ sin θ sin ψ -cos θ sin ψ sin γ cos ψ + cos γ sin θ sin ψ ];

if the matrix B is full, ▽_x、▽_yAnd ▽_zUniquely determined by equation (30):

${\left[\begin{array}{ccc}{\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z\end{array}\right]}^T=B^{-1}{\left[\begin{array}{ccc}{\&dtri;}_U^I& {\&dtri;}_U^{II}& {\&dtri;}_U^{III}\end{array}\right]}^T---\left(30\right)$

at this time, the process of the present invention,andobtained from formula (31):

${\left[\begin{array}{cc}{\&dtri;}_E^{III}& {\&dtri;}_N^{III}\end{array}\right]}^T=D{\left[\begin{array}{ccc}{\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z\end{array}\right]}^T---\left(31\right)$

wherein

Alignment at the third position, according to the estimatedAndin PX estimated by formula (9), the calculation is performed according to formula (32) to obtain the misalignment angle phi at the alignment end time_E、φ_NAnd phi_U；

$\begin{array}{c}{\mathrm{\&phi;}}_E={\left(PX\right)}_5-\frac{1}{g}{\&dtri;}_N\\ {\mathrm{\&phi;}}_N={\left(PX\right)}_6+\frac{1}{g}{\&dtri;}_E\\ {\mathrm{\&phi;}}_U={\left(PX\right)}_7-\frac{1}{{\mathrm{\&omega;}}_{ie}\cos L}{\mathrm{\&epsiv;}}_E+\frac{\tan L}{g}{\&dtri;}_E\end{array}---\left(32\right)$

2. The method of claim 1, wherein in step 2, if a single-position alignment method is used, then phi is_EAlignment error ofφ_NAlignment error ofφ_UAlignment error ofEstimated according to step 4 Andto phi_E、φ_NAnd phi_UThe result of the alignment is compensated, and the misalignment angle phi at the alignment end time is calculated_E、φ_NAnd phi_UThen, the attitude matrix of the third position is updated according to equation (33)

${\stackrel{\&OverBar;}{C}}_{b3}^n=\&lsqb;I+(\mathrm{\&phi;}\&times;)\&rsqb;C_{b3}^n---\left(33\right)$

Namely the final attitude matrix after error compensation.