CN102620748B - Method for estimating and compensating lever arm effect in case of shaken base by strapdown inertial navigation system - Google Patents

Abstract

The invention provides a method for estimating and compensating a lever arm effect in the case of a shaken base by a strapdown inertial navigation system, which is used for estimating the influence of the lever arm effect to the system and compensating according to a certain strategy, so that the precision of fine alignment and navigation calculation can be improved. The method comprises the following steps of: firstly, on the basis of coarse alignment, dividing a fine alignment process into two stages, wherein at a first stage: running a parameter identification method fine alignment algorithm of lever arm speed real-time compensating and spreading variable quantity, to estimate the residual interference speed except the lever arm speed in the system, and at a second stage: carrying out primary compensation on the residual interference speed; continuously running the parameter identification method fine alignment algorithm of the lever arm speed real-time compensating and spreading variable quantity, to estimate the misalignment angle information; and further modifying a coarse alignment result by the estimated misalignment angle, and confirming an initial pose matrix, so that the fine alignment can be completed. In the stage of navigation calculation, the lever arm speed is compensated in real time, a strapdown calculating program is operated, and a navigation result is provided.

Description

Estimation and the compensation method of lever arm effect under strapdown inertial navitation system (SINS) swaying base condition

Technical field

The invention belongs to inertial navigation technical field, relate to inertial navigation system, is under a kind of strapdown inertial navitation system (SINS) swaying base condition, estimation and the compensation method of the velocity error being caused by lever arm effect in fine alignment and navigation stage.

Background technology

Inertial technology is one of technical field of the each industrial powers development in the world, inertial navigation system (INS, be called for short inertial navigation) be a kind of system of utilizing inertial technology to realize carrier independent navigation, inertial navigation does not have entity platform, and the interference of rocking of carrier directly adds to gyroscope and accelerometer.Initial alignment is one of strapdown inertial navitation system (SINS) gordian technique, and it provides the initial value resolving for navigational system, and the precision of navigational system is had a great impact, and becomes in recent years the focus of Chinese scholars research.External interference can make sensor output signal introduce noise, affects the reliability of sensor signal, and then impact is aimed at and navigation accuracy.

Lever arm effect is owing to do not overlap with carrier swing center in inertial measurement cluster installation site in the situation that, carrier is subject to external interference or carrier movement makes strapdown inertial navitation system (SINS) pedestal in waving or vibrating state, cause accelerometer output information to be interfered, and then affect velocity information.The interference that lever arm effect causes can cause the error that navigational system initial alignment and navigation calculation are very large, must estimate and compensate.

Because lever arm length computation is inaccurate, the factor such as the angular speed of carrier movement and large, the initial lever arm velocity error meter difficult to estimate of angle rate of acceleration estimating noise, traditional lever arm speed Dynamics Compensation method can not full remuneration lever arm error, has remnants.Cause high frequency interference by low pass filtering method filtering lever arm effect, can cause that useful information is lost, degradation problem under asynchronous, the alignment precision of sensor information.Design lever arm effect estimation new under a kind of swaying base condition and the method for compensation and there is important engineering practical value.

Summary of the invention

The problem to be solved in the present invention is: initial alignment and the navigation calculation of lever arm effect to strapdown inertial navitation system (SINS) has a great impact, and existing technological means can not be to lever arm effect effective compensation.The present invention mainly solves under swaying base condition, how to estimate lever arm effect and how to compensate the problem of its impact at aligning and navigation stage.

Technical scheme of the present invention is: estimation and the compensation method of lever arm effect under a kind of strapdown inertial navitation system (SINS) swaying base condition, and estimate the impact of lever arm effect on strapdown inertial navitation system (SINS) and compensate, comprise the following steps:

1) strapdown inertial navitation system (SINS) start preheating, the output data of collection inertial measurement cluster;

2) carry out coarse alignment, obtain rough initial attitude matrix

3) on coarse alignment basis, point two stages complete fine alignment process;

31) set up the parameter identification method fine alignment mathematical model of expansion variable, tectonic system equation and observation equation; The parameter identification method fine alignment mathematical model of expansion variable:

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=({\▿}_e-g.{\mathrm{\φ}}_{n0})t-\frac{t^2}{2}g{u}_n+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=({\▿}_n+g.{\mathrm{\φ}}_{e0})t+\frac{t^2}{2}g{u}_e+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}++V_{\mathrm{sn}}\end{array}\right.$

Wherein $\left\{\begin{array}{c}{u}_e={\mathrm{\φ}}_{n0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\φ}}_{u0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_e\\ u_n=-{\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\ϵ}}_n\\ u_u={\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_u\end{array}\right.$

In formula: represent respectively the normal value biasing of accelerometer equivalence east orientation and north orientation; φ _e0, φ _n0, φ _u0represent initial misalignment; ω _e, ω _n, ω _urepresent that equivalence east, north, sky are to gyroscope constant value drift; ω _ierepresent earth rotation angular speed; L represents local latitude; G represents terrestrial gravitation acceleration; V _se, V _snrepresent respectively east orientation and north orientation random disturbance speed, V _de, V _dnrepresenting east orientation and north orientation residual interference speed, is normal value; Δ V _e, Δ V _nrepresent respectively east orientation and north orientation velocity error, that the speed resolved of strapdown inertial navitation system (SINS) is rejected the poor of velocity amplitude that value after lever arm speed and outside reference provide, under swaying base condition, the speed that outside reference provides is 0m/s, the process of rejecting lever arm speed the speed of resolving from strapdown inertial navitation system (SINS) is the real-Time Compensation of lever arm speed, and the lever arm speed being caused by lever arm effect calculates according to lever arm rate pattern:

$\mathrm{\δ}{v}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right)$

In formula: δ v _grepresent lever arm speed; R=(r _x, r _y, r _z) indication rod arm lengths vector, this value is calculated in advance and is set into system according to the project organization of carrier and navigational system installation site, and in practical application, lever arm length vector can be because the factors such as carrier deflection deformation, load changes in distribution depart from this value; represent the gyroscope output angle speed in three directions;

Described residual interference speed comprises: the random disturbance speed that system exists; Because of the lever arm speed error of calculation that lever arm linear measure longimetry is inaccurate, carrier exists deflection deformation, the angular speed of gyroscope survey exists the factors such as interference to cause; Under swaying base condition, strapdown resolves in the speed of initial time and has lever arm speed, and when operation strapdown algorithm, initial velocity press 0m/s processing, the residual speed causing; After coarse alignment, there is error in initial attitude matrix, when operation strapdown algorithm, and the velocity error of bringing;

According to the parameter identification method mathematical model of expansion variable, tectonic system equation and observation equation are:

Taking strapdown inertial navitation system (SINS) medium velocity error as observed quantity, the parameter identification method mathematical model of expansion variable is rewritten into following form:

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.,K=\mathrm{0,1,2},\·\·\·$

In formula: the sensor data samples cycle that T is strapdown inertial navitation system (SINS);

$\left\{\begin{array}{c}{a}_{1e}=({\▿}_e-g.{\mathrm{\φ}}_{n0}),a_{2e}=-\frac{1}{2}g{u}_n,a_{3e}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\▿}_n+g.{\mathrm{\φ}}_{e0}),a_{2n}=\frac{1}{2}g{u}_e,a_{3n}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

East orientation and north orientation velocity error Δ V _e, Δ V _eas observed quantity, a _1e, a _2e, a _3e, V _de, a _1n, a _2n, a _3n, V _dnas parameter to be identified, tectonic system equation and observation equation:

Define parameter to be identified, system state variables is:

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ V_{\mathrm{de}}\end{array}\right],$ $X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

List system equation and observation equation:

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\Δ}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,$ $\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\Δ}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

In formula: observing matrix H (k)=[kT, (kT) ², (kT) ³, 1]; V _ge(k), V _gn(k) the speed observation noise of expression east orientation and north orientation, its variance intensity is R _e, R _n;

32) the fine alignment first stage, according to coarse alignment result, operation strapdown resolves program, carries out the real-Time Compensation of lever arm speed, simultaneously according to step 31) system equation and the observation equation estimating system state variable set up, the 4th component of state variable is residual interference speed;

33) fine alignment subordinate phase: residual interference speed is carried out to single compensation; Then, continue the lever arm speed real-Time Compensation of operation fine alignment first stage and the algorithm for estimating to system state variables, and carry out initial misalignment estimation, after the initial misalignment convergence of estimating, by estimated value φ _e0, φ _n0, φ _u0in substitution misalignment Changing Pattern model, calculate the misalignment φ of current time _e, φ _n, φ _u; Utilize the misalignment information of current time to coarse alignment result carry out a step correction, obtain the attitude matrix of current time pass through attitude matrix extract position angle H, pitch angle P and roll angle R, fine alignment completes;

Wherein, according to step 32) estimate that the state variable that obtains calculates u _e, u _n, u _u, φ _e0, φ _n0and φ _u0:

$u_e=\frac{2a_{2n}}{g},$ $u_n=-\frac{2a_{2e}}{g},$ $u_u=-\frac{6a_{3n}}{g{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\φ}}_{e0}=\frac{a_{1n}}{g},$ ${\mathrm{\φ}}_{n0}=-\frac{a_{1e}}{g},$ ${\mathrm{\φ}}_{u0}={\mathrm{\φ}}_{n0}.\tan L-\frac{u_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$

Calculate current time misalignment according to initial misalignment and misalignment Changing Pattern model:

$\left\{\begin{array}{c}{\mathrm{\φ}}_e={\mathrm{\φ}}_{e0}+u_e.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\\ {\mathrm{\φ}}_n={\mathrm{\φ}}_{n0}+u_n.t-\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ {\mathrm{\φ}}_u={\mathrm{\φ}}_{u0}+u_u.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\cos L\end{array}\right.$

To coarse alignment result a step be modified to:

$C_b^n=C_{n^{\′}}^n{C}_b^{n^{\′}}=[I+(\mathrm{\φ}\×)]C_b^{n^{\′}},$ Wherein $C_{n^{\′}}^n=I+(\mathrm{\φ}\×)=\left(\begin{array}{ccc}0& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right)$

4) the navigation calculation stage, carry out the real-Time Compensation of lever arm speed, according to step 33) attitude matrix that obtains operation strapdown algorithm, provides navigation results.

Step 32) in, adopt improvement kalman filter method estimating system state variable:

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right),i=e,n;k=\mathrm{0,1,2}\·\·\·\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\Δ}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.$

The corresponding system state variables X that represents of i _e, X _nin subscript e, n, original state variable X _i(0), Initial state estimation error covariance matrix P _iand initial observation noise variance intensity R (0) _i(0) value all can be optional,

Above formula is estimated to system state variables X with recursive algorithm _e, X _n.

Step 32), 33) and 4) in, the real-Time Compensation of lever arm speed is: carry out strapdown while resolving, each speed is rejected in the velocity information the lever arm speed of current time from upgrading after upgrading, and the lever arm speed being caused by lever arm effect calculates according to lever arm rate pattern:

$\mathrm{\δ}{v}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right).$

Step 33) in, residual interference speed is carried out to single compensation method is: after the fine alignment first stage finishes, the residual interference speed estimating is normal value, and fine alignment subordinate phase at the beginning, the velocity information that remaining disturbance velocity is calculated from strapdown, reject, method is:

$\left\{\begin{array}{c}{V^{\′}}_e=V_e-V_{\mathrm{de}}\\ {V^{\′}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

In formula: V ' _e, V ' _nrepresent to reject east orientation and the north orientation speed after residual interference speed; V _e, V _nrepresent east orientation and north orientation speed that strapdown resolves; V _de, V _dnrepresent east orientation and north orientation residual interference speed.

Also have at present the part research report relevant with the present invention, wherein parameter identification method fine alignment of the present invention is with reference to two sections of articles below: 1, strapdown inertial navitation system (SINS) modified parameters identification Initial Alignment Method, Chinese inertial technology journal, 2010,18 (5); 2, the fast and accuracy alignment method of SINS on swaying base, BJ University of Aeronautics & Astronautics's journal, 2009,35 (1); Lever arm rate pattern is with reference to following patent: 3, number of patent application is CN201010270972.4, and name is called " alignment methods of eliminating underwater vehicle strapdown inertial navitation system (SINS) lever arm effect error ".The present invention is different from prior art and is characterised in that: 1, fine alignment process is divided into two stages, the first stage is estimated residual interference speed, subordinate phase to residual interference speed single compensation after, estimate misalignment, coarse alignment result is revised.2,, in the fine alignment stage, the real-Time Compensation to lever arm effect and the single compensation to residual interference speed mutually combine, and can improve the compensation efficiency of lever arm effect.3, the error source of labor residual interference speed, has designed the parameter identification method fine alignment algorithm of expansion variable targetedly, the residual interference speed in system that estimates that can be relatively accurate.4,, in whole alignment procedures, keep the real-Time Compensation to lever arm speed.

The present invention proposes estimation and the compensation method of the velocity error that under a kind of strapdown inertial navitation system (SINS) swaying base condition, fine alignment and navigation stage are caused by lever arm effect, object is estimate the impact of lever arm effect on system and compensate by certain strategy, to improve the precision of fine alignment and navigation calculation.Advantage of the present invention is: adopt the parameter identification method fine alignment algorithm of expansion variable, calculated amount is little, and precision is high; Can estimate the system residual disturbance velocity after Dynamics Compensation lever arm speed; Fine alignment algorithm is divided into two stages, and the first stage finishes rear residual interference speed to be carried out to single compensation, can improve subordinate phase fine alignment precision; In the navigation calculation stage, lever arm speed is carried out to real-Time Compensation, can improve navigation calculation precision.

Brief description of the drawings

Fig. 1 is process flow diagram of the present invention.

Fig. 2 is lever arm effect schematic diagram.

Fig. 3 is that lever arm effect of the present invention is estimated and backoff algorithm process flow diagram.

Fig. 4 is the misalignment curve map that the parameter identification method fine alignment algorithm of first group of expansion variable of the present invention is estimated.

Fig. 5 is the residual interference speed curve diagram that the parameter identification method fine alignment algorithm of first group of expansion variable of the present invention is estimated.

Fig. 6 is the misalignment curve map that the parameter identification method fine alignment algorithm of second group of expansion variable of the present invention is estimated.

Fig. 7 is the residual interference speed curve diagram that the parameter identification method fine alignment algorithm of second group of expansion variable of the present invention is estimated.

Embodiment

Implementation process of the present invention is done to detailed description below, flow process as shown in Figure 1.

First define coordinate system.Definition ' sky, northeast ' coordinate is navigation coordinate system, is designated as n system; Carrier coordinate system b is taking carrier center of gravity as initial point, and X-axis is pointed to right along transverse axis, before Y-axis is pointed to along the longitudinal axis, on Z axis vertical carrier points to; Terrestrial coordinate system e system, coordinate origin is at the earth's core point, and X-axis is plane under the line, points to the first meridian, and Y-axis is vertical with X-axis, and also under the line in plane, Z axis is determined by right hand rule, sensing earth rotation direction of principal axis.Geocentric inertial coordinate system i system, this coordinate system overlaps at initial time and terrestrial coordinate system.

Step 1) gather the output data of inertial measurement cluster after strapdown inertial navitation system (SINS) start preheating;

Step 2) carry out coarse alignment, obtain rough initial attitude matrix because rapidity is the performance index that inertial navigation system is aimed at, move 3-5 minute coarse alignment algorithm, can obtain rough attitude matrix;

Step 3) on coarse alignment basis, divide two stages to complete fine alignment process, its flow process is as shown in Figure 3;

Step 31) set up the parameter identification method fine alignment mathematical model of expansion variable, tectonic system equation and observation equation;

The parameter identification method mathematical model of model expansion variable;

On swaying base, the misalignment Changing Pattern between coarse alignment definite navigation system and the navigation system of theory is as follows:

$\left\{\begin{array}{c}{\mathrm{\φ}}_e={\mathrm{\φ}}_{e0}+u_e.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\\ {\mathrm{\φ}}_n={\mathrm{\φ}}_{n0}+u_n.t-\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ {\mathrm{\φ}}_u={\mathrm{\φ}}_{u0}+u_u.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\cos L\end{array}\right.$ Wherein $\left\{\begin{array}{c}{u}_e={\mathrm{\φ}}_{n0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\φ}}_{u0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_e\\ u_n=-{\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\ϵ}}_n\\ u_u={\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_u\end{array}\right.$

Specific force equation in equivalent level direction is:

$\left\{\begin{array}{c}{f_e}^{n^{\′}}=-g.{\mathrm{\φ}}_n+f_{\mathrm{de}}+{\▿}_e\\ {f_n}^{n^{\′}}=g.{\mathrm{\φ}}_e+f_{\mathrm{dn}}+{\▿}_n\end{array}\right.$

In formula: f _de, f _dnbe respectively the disturbing acceleration of equivalent east orientation and equivalent north orientation.As can be seen from the above equation, owing to there being misalignment φ _e, φ _n, φ _u, terrestrial gravitation acceleration information g causes being coupled in the ratio force information of horizontal direction.Specific force in horizontal direction is quadratured in the time period at [0, t], obtain the speed of accumulation in this time period.Because carrier is on swaying base, wireless motion, so this velocity amplitude is also speed error value, its expression formula is:

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=({\▿}_e-g.{\mathrm{\φ}}_{n0})t-\frac{t^2}{2}g{u}_n+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=({\▿}_n+g.{\mathrm{\φ}}_{e0})t+\frac{t^2}{2}g{u}_e+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}++V_{\mathrm{sn}}\end{array}\right.$

When strapdown inertial navitation system (SINS) installation site is not during at swing center, lever arm effect can impact east orientation and north orientation speed, and its principle as shown in Figure 2.In formula above, represent respectively the normal value biasing of accelerometer equivalence east orientation and north orientation; φ _e0, φ _n0, φ _u0represent initial misalignment; ε _e, ε _n, ε _urepresent that equivalence east, north, sky are to gyroscope constant value drift; ω _ierepresent earth rotation angular speed; L represents local latitude; G represents terrestrial gravitation acceleration; V _se, V _snrepresent respectively east orientation and north orientation random disturbance speed, V _de, V _dnrepresenting east orientation and north orientation residual interference speed, is normal value; Δ V _e, Δ V _nrepresent respectively east orientation and north orientation velocity error, that the speed resolved of strapdown inertial navitation system (SINS) is rejected the poor of velocity amplitude that value after lever arm speed and outside reference provide, under swaying base condition, the speed that outside reference provides is 0m/s, and the process of rejecting lever arm speed the speed of resolving from strapdown inertial navitation system (SINS) is the real-Time Compensation of lever arm speed.If lever arm length vector is r=(r _x, r _y, r _z), the lever arm rate pattern being caused by lever arm effect is:

$\mathrm{\δ}{v}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right)$

In formula: δ v _grepresent lever arm speed; R=(r _x, r _y, r _z) indication rod arm lengths vector, this value is calculated in advance and is set into system according to the project organization of carrier and navigational system installation site, in practical application, lever arm length vector can depart from this value because of factors such as carrier deflection deformation, load changes in distribution, and then produces error; represent the gyroscope output angle speed in three directions; Therefore labor of the present invention the error source of residual interference speed, described residual interference speed comprises: system exist random disturbance speed; Because of the lever arm speed error of calculation that lever arm linear measure longimetry is inaccurate, carrier exists deflection deformation, the angular speed of gyroscope survey exists the factors such as interference to cause; Under swaying base condition, strapdown resolves in the speed of initial time and has lever arm speed, and when operation strapdown algorithm, initial velocity press 0m/s processing, the residual speed causing; After coarse alignment, there is error in initial attitude matrix, when operation strapdown algorithm, and the velocity error of bringing.

Parameter identification method mathematical model tectonic system equation and observation equation according to expansion variable:

Using strapdown inertial navitation system (SINS) medium velocity error as observed quantity, the parameter identification method mathematical model of expansion variable is rewritten into following form:

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.$

In formula: the sensor data samples cycle that T is strapdown inertial navitation system (SINS);

$\left\{\begin{array}{c}{a}_{1e}=({\▿}_e-g.{\mathrm{\φ}}_{n0}),a_{2e}=-\frac{1}{2}g{u}_n,a_{3e}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\▿}_n+g.{\mathrm{\φ}}_{e0}),a_{2n}=\frac{1}{2}g{u}_e,a_{3n}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

East orientation and north orientation velocity error Δ V _e, Δ V _nas observed quantity, a _1e, a _2e, a _3e, V _de, a _1n, a _2n, a _3n, V _dnregard parameter to be identified as, tectonic system equation and observation equation:

Define parameter to be identified, system state variables is:

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ V_{\mathrm{de}}\end{array}\right],$ $X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

According to analysis above, list system equation and observation equation:

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\Δ}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,$ $\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\Δ}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

In formula: observing matrix H (k)=[kT, (kT) ², (kT) ³, 1]; V _ge(k), V _gn(k) the speed observation noise of expression east orientation and north orientation, its variance intensity is R _e, R _n.

Step 32) the fine alignment first stage, according to coarse alignment result, operation strapdown resolves program, carries out the real-Time Compensation of lever arm speed, simultaneously according to step 31) system equation and the observation equation estimating system state variable set up, the 4th component of state variable is residual interference speed.The method of estimated state variable is a lot, can pass through least square method recursion or kalman filter method, and the present invention adopts improvement kalman filter method estimated state variable.Based on the consideration of rapidity and precision, this process is generally made as 3-5 minute.

The method of the real-Time Compensation of the speed of lever arm described in the present invention is: carry out strapdown while resolving, after each speed is upgraded, in velocity information the lever arm speed of current time from upgrading, reject, the lever arm speed being caused by lever arm effect calculates according to lever arm rate pattern:

$\mathrm{\δ}{v}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right)$

State variable X _e, X _nvalue can calculate as recursion by least square method.In recursive process, parameter is gradually to true value convergence, and this process is from observation information, to refine the concentration process of estimated value, and along with recursion is more deep, concentration process is more effective, and estimated value also more approaches actual value.

The present invention has adopted improvement kalman filter method to estimate to the estimation of state variable, and algorithm for estimating is as follows:

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right),i=e,n;k=\mathrm{0,1,2}\·\·\·\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\Δ}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.$

I correspondence system state variable X _e, X _nin subscript e, n.Original state variable X _i(0), the variance battle array P of Initial state estimation error _iand the variance intensity R of initial observation noise (0) _i(0) value all can be optional.Might as well get X _i(0)=0; P _i(0)=I α, α is positive scalar arbitrarily; R _i(0)=0.2.In algorithm, calculation of filtered gain battle array K _i(k) time, utilized new breath e _i(k), the benefit of doing is like this: even the observation noise statistical property the unknown in system, also can be according to the adaptive observation noise that calculates current time of new breath value, and upgrade filter gain battle array according to observation noise, be conducive to the Fast Convergent of algorithm.

Can estimate system state variables X to above formula with recursive algorithm _e, X _n.

Step 33) fine alignment subordinate phase: residual interference speed is carried out to single compensation; Then, continue the lever arm speed real-Time Compensation of operation fine alignment first stage and the algorithm for estimating to system state variables, carry out initial misalignment estimation, after the initial misalignment convergence of estimating, by estimated value φ _e0, φ _n0, φ _u0in substitution misalignment Changing Pattern model, calculate the misalignment φ of current time _e, φ _n, φ _u; Utilize the misalignment information of current time to coarse alignment result carry out a step correction, obtain the attitude matrix of current time pass through attitude matrix extract position angle H, pitch angle P and roll angle R, fine alignment completes.Based on the consideration of rapidity and precision, this process is generally made as 5 minutes.Specific as follows:

After system state variables estimates, calculate u _e, u _n, u _u, φ _e0, φ _n0and φ _u0:

$u_e=\frac{2a_{2n}}{g},$ $u_n=-\frac{2a_{2e}}{g},$ $u_u=-\frac{6a_{3n}}{g{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\φ}}_{e0}=\frac{a_{1n}}{g},$ ${\mathrm{\φ}}_{n0}=-\frac{a_{1e}}{g},$ ${\mathrm{\φ}}_{u0}={\mathrm{\φ}}_{n0}.\tan L-\frac{u_e+{\mathrm{\ϵ}}_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$

Because equivalent east orientation gyroscopic drift ε _ethe unknown, day to the following formula approximate treatment of the initial value of misalignment:

${\mathrm{\φ}}_{u0}\≈{\mathrm{\φ}}_{n0}.\tan L-\frac{u_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$

Owing to having ignored the constant value drift of sensor in calculating, be there is to error in the initial estimate of misalignment, the expression formula of error amount is:

$\left\{\begin{array}{c}\mathrm{\δ}{\mathrm{\φ}}_{e0}=-\frac{{\▿}_n}{g}\\ \mathrm{\δ}{\mathrm{\φ}}_{n0}=\frac{{\▿}_e}{g}\\ \mathrm{\δ}{\mathrm{\φ}}_{u0}=-\frac{{\mathrm{\ϵ}}_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}\end{array}\right.$

After coarse alignment finishes, the navigation of being set up by navigational computer is that n ' is that n exists misalignment φ with desirable navigation _e, φ _n, φ _u, they are all little angles, vector φ=(φ _eφ _nφ _u) can be regarded as n and be tied to the equivalent rotating vector that n ' is, n is tied to the attitude transition matrix that n ' is and can be expressed from the next:

$C_{n^{\′}}^n=I+(\mathrm{\φ}\×)=\left(\begin{array}{ccc}0& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right)$

To coarse alignment result a step be modified to:

$C_b^n=C_{n^{\′}}^n{C}_b^{n^{\′}}=[I+(\mathrm{\φ}\×)]C_b^{n^{\′}},$ Wherein $C_{n^{\′}}^n=I+(\mathrm{\φ}\×)=\left(\begin{array}{ccc}0& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right)$

Residual interference speed is carried out to single compensation method is: after the fine alignment first stage finishes, the residual interference speed estimating is normal value, and fine alignment subordinate phase at the beginning, is rejected the velocity information that remaining disturbance velocity is calculated from strapdown, and method is:

$\left\{\begin{array}{c}{V^{\′}}_e=V_e-V_{\mathrm{de}}\\ {V^{\′}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

In formula: V ' _e, V ' _nrepresent to reject east orientation and the north orientation speed after residual interference speed; V _e, V _nrepresent east orientation and north orientation speed that strapdown resolves; V _de, V _dnrepresent east orientation and north orientation residual interference speed.

Step 4) the navigation calculation stage, carry out the real-Time Compensation of lever arm speed, according to step 33) attitude matrix that obtains operation strapdown algorithm, provides navigation results.

Method of the present invention is carried out to emulation experiment:

Simulated conditions: carrier is under horizontal jitter pedestal condition, and shaking amplitude and frequency are respectively: 0.17 °/1Hz; In system, consider lever arm impact, lever arm length is respectively: 0.456m ,-0.456m, 22.546m; The gyroscope constant value drift of three directions is 0.15 °/h, and Jia Biaochang value is biased to 0.11mg, and latitude is 31183 °, and attitude of carrier is: 2 ° of the angles of pitch, 3 ° of roll angles.After coarse alignment finishes, be the parameter identification method fine alignment that expansion variable is carried out in the position of 90 ° in course, in order to verify the estimated accuracy to system residual disturbance velocity, carried out two groups of experiments.Fine alignment first stage algorithm is carried out in first group of experiment, moves 5 minutes; Fine alignment subordinate phase algorithm is carried out in second group of experiment, moves 5 minutes.

First group of experiment: setting up departments system there is initial velocity error, there is residual interference speed, operation expansion variable parameter identification method fine alignment program after, estimate initial interference speed, alignment result as shown in Figure 4, Figure 5:

First group of experimental result: coarse alignment result is: course error δ H=-38.76 ', δ P=0.61 ', δ R=-0.21 ', the distance dis (C)=0.016 of the attitude matrix after aligning and theoretical attitude matrix.The accurate result of parameter identification method is: course error δ H=-42 ', δ P=0.17 ', δ R=-20 ', the distance dis (C)=0.017 of the attitude matrix after aligning and theoretical attitude matrix.Because when first group of experiment, there is initial disturbance velocity in system, has residual interference speed, causes fine alignment process to have disturbing effect.The east orientation disturbance velocity that the method is estimated is 0.4138m/s, be approximately-0.4245m/s of north orientation disturbance velocity.

Second group of experiment carried out single compensation the disturbance velocity that estimates in fine alignment subordinate phase on the basis of last group of experiment, alignment result as shown in Figure 6, Figure 7:

Second group of accurate result of experiment parameter identification method is: course error δ H=-38.94 ', and δ P=-0.15 ', δ R=-0.20 ', the distance dis (C)=0.0016 of the attitude matrix after aligning and theoretical attitude matrix, compares coarse alignment and first stage fine alignment result, and precision increases.Because second group of emulation experiment compensates the initial interference speed of first group of experiment estimation, misalignment evaluated error very rapid convergence, to smaller value, has improved alignment precision.According to experimental result, illustrate that the initial lever arm disturbance velocity of first group of experiment estimation is effective.In second group of experiment, because residual interference speed is estimated and compensated, remaining disturbance velocity theoretical in system is: east orientation 0m/s, north orientation 0m/s.In this group test, disturbance velocity remaining in system to be estimated, estimated result is: east orientation disturbance velocity is 0.00045m/s, be approximately-0.0006m/s of north orientation disturbance velocity, and theoretical 0m/s, 0m/s are very approaching, estimated accuracy exceedes 95%.

Claims (1)

1. estimation and the compensation method of lever arm effect under strapdown inertial navitation system (SINS) swaying base condition, is characterized in that under swaying base condition, estimates the impact of lever arm effect on strapdown inertial navitation system (SINS) and compensates, and comprises the following steps:

1) strapdown inertial navitation system (SINS) start preheating, the output data of collection inertial measurement cluster;

2) carry out coarse alignment, obtain rough initial attitude matrix

3) on coarse alignment basis, point two stages complete fine alignment process;

31) set up the parameter identification method fine alignment mathematical model of expansion variable, tectonic system equation and observation equation; The parameter identification method fine alignment mathematical model of expansion variable:

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=({\▿}_e-g\·{\mathrm{\φ}}_{n0})t-\frac{t^2}{2}{\mathrm{gu}}_n+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=({\▿}_n+g\·{\mathrm{\φ}}_{e0})t+\frac{t^2}{2}{\mathrm{gu}}_e+\frac{t^3}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.$

Wherein $\left\{\begin{array}{c}{u}_e={\mathrm{\φ}}_{n0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\φ}}_{u0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_e\\ u_n=-{\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\sin L-{\mathrm{\ϵ}}_n\\ u_u={\mathrm{\φ}}_{e0}{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\ϵ}}_u\end{array}\right.$

In formula: represent respectively the normal value biasing of accelerometer equivalence east orientation and north orientation; φ _e0, φ _n0, φ _u0represent initial misalignment; ε _e, ε _n, ε _urepresent that equivalence east, north, sky are to gyroscope constant value drift; ω _ierepresent earth rotation angular speed; L represents local latitude; G represents terrestrial gravitation acceleration; V _se, V _snrepresent respectively east orientation and north orientation random disturbance speed, V _de, V _dnrepresenting east orientation and north orientation residual interference speed, is normal value; Δ V _e, Δ V _nrepresent respectively east orientation and north orientation velocity error, that the speed resolved of strapdown inertial navitation system (SINS) is rejected the poor of velocity amplitude that value after lever arm speed and outside reference provide, under swaying base condition, the speed that outside reference provides is 0m/s, the process of rejecting lever arm speed the speed of resolving from strapdown inertial navitation system (SINS) is the real-Time Compensation of lever arm speed, and the lever arm speed being caused by lever arm effect calculates according to lever arm rate pattern:

${\mathrm{\δv}}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right)$

In formula: δ v _grepresent lever arm speed; R=(r _x, r _y, r _z) indication rod arm lengths vector, this value is calculated in advance and is set into system according to the project organization of carrier and navigational system installation site, and in practical application, lever arm length vector can be because the factors such as carrier deflection deformation, load changes in distribution depart from this value; represent the gyroscope output angle speed in three directions;

Described residual interference speed comprises: the random disturbance speed that system exists; Because of the lever arm speed error of calculation that lever arm linear measure longimetry is inaccurate, carrier exists deflection deformation, the angular speed of gyroscope survey exists the factors such as interference to cause; Under swaying base condition, strapdown resolves in the speed of initial time and has lever arm speed, and when operation strapdown algorithm, initial velocity press 0m/s processing, the residual speed causing; After coarse alignment, there is error in initial attitude matrix, when operation strapdown algorithm, and the velocity error of bringing;

According to the parameter identification method mathematical model of expansion variable, tectonic system equation and observation equation are:

Taking strapdown inertial navitation system (SINS) medium velocity error as observed quantity, the parameter identification method mathematical model of expansion variable is rewritten into following form:

$\left\{\begin{array}{c}\mathrm{\Δ}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\Δ}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.,K=\mathrm{0,1,2},...$

In formula: the sensor data samples cycle that T is strapdown inertial navitation system (SINS);

$\left\{\begin{array}{c}{a}_{1e}=({\▿}_e-g\·{\mathrm{\φ}}_{n0}),a_{2e}=-\frac{1}{2}{\mathrm{gu}}_n,a_{3e}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\▿}_n+g\·{\mathrm{\φ}}_{e0}),a_{2n}=\frac{1}{2}{\mathrm{gu}}_e,a_{3n}=\frac{1}{6}g{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

East orientation and north orientation velocity error △ V _e, △ V _nas observed quantity, a _1e, a _2e, a _3e, V _de, a _1n, a _2n, a _3n, V _dnas parameter to be identified, tectonic system equation and observation equation:

Define parameter to be identified, system state variables is:

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ a_{\mathrm{de}}\end{array}\right],X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

List system equation and observation equation:

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\Δ}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\Δ}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

In formula: observing matrix H (k)=[kT, (kT) ², (kT) ³, 1]; V _ge(k), V _gn(k) the speed observation noise of expression east orientation and north orientation, its variance intensity is R _e, R _n;

32) the fine alignment first stage, according to coarse alignment result, operation strapdown resolves program, carries out the real-Time Compensation of lever arm speed, simultaneously according to step 31) system equation and the observation equation estimating system state variable set up, the 4th component of state variable is residual interference speed; Wherein, adopt improvement kalman filter method estimating system state variable:

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right)\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\Δ}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.,i=e,n;k=\mathrm{0,1,2}...$

The corresponding system state variables X that represents of i _e, X _nin subscript e, n, original state variable X _i(0), Initial state estimation error covariance matrix P _iand initial observation noise variance intensity R (0) _i(0) value all can be optional,

Above formula is estimated to system state variables X with recursive algorithm _e, X _n;

33) fine alignment subordinate phase: residual interference speed is carried out to single compensation, method is: after the fine alignment first stage finishes, the residual interference speed estimating is normal value, and fine alignment subordinate phase at the beginning, the velocity information that remaining disturbance velocity is calculated from strapdown, reject, method is:

$\left\{\begin{array}{c}{V^{\′}}_e=V_e-V_{\mathrm{de}}\\ {V^{\′}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

In formula: V' _e, V' _nrepresent to reject east orientation and the north orientation speed after residual interference speed; V _e, V _nrepresent east orientation and north orientation speed that strapdown resolves; V _de, V _dnrepresent east orientation and north orientation residual interference speed;

Then, continue the lever arm speed real-Time Compensation of operation fine alignment first stage and the algorithm for estimating to system state variables, and carry out initial misalignment estimation, after the initial misalignment convergence of estimating, by estimated value φ _e0, φ _n0, φ _u0in substitution misalignment Changing Pattern model, calculate the misalignment φ of current time _e, φ _n, φ _u; Utilize the misalignment information of current time to coarse alignment result carry out a step correction, obtain the attitude matrix of current time pass through attitude matrix extract position angle H, pitch angle P and roll angle R, fine alignment completes;

Wherein, according to step 32) estimate that the state variable that obtains calculates u _e, u _n, u _u, φ _e0, φ _n0and φ _u0:

$u_e=\frac{2a_{2n}}{g},u_n=-\frac{2a_{2e}}{g},u_u=-\frac{6a_{3n}}{g{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\φ}}_{e0}=\frac{a_{1n}}{g},{\mathrm{\φ}}_{n0}=-\frac{a_{1e}}{g},{\mathrm{\φ}}_{u0}={\mathrm{\φ}}_{n0}.\tan L-\frac{u_e}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$

Calculate current time misalignment according to initial misalignment and misalignment Changing Pattern model:

$\left\{\begin{array}{c}{\mathrm{\φ}}_e={\mathrm{\φ}}_{e0}+u_e.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\\ {\mathrm{\φ}}_n={\mathrm{\φ}}_{n0}+u_n.t-\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\sin L\\ {\mathrm{\φ}}_u={\mathrm{\φ}}_{u0}+u_u.t+\frac{t^2}{2}.{\mathrm{\ω}}_{\mathrm{ie}}{u}_e\cos L\end{array}\right.$

To coarse alignment result a step be modified to:

$C_b^n=C_{n^{\′}}^n{C}_b^{n^{\′}}=[I+(\mathrm{\φ}\×)]C_b^{n^{\′}},$ Wherein $C_{n^{\′}}^n=I+(\mathrm{\φ}\×)=\left(\begin{array}{ccc}0& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right)$

4) the navigation calculation stage, carry out the real-Time Compensation of lever arm speed, according to step 33) attitude matrix that obtains operation strapdown algorithm, provides navigation results;

Step 32), 33) and 4) in, the real-Time Compensation of lever arm speed is: carry out strapdown while resolving, each speed is rejected in the velocity information the lever arm speed of current time from upgrading after upgrading, and the lever arm speed being caused by lever arm effect calculates according to lever arm rate pattern:

${\mathrm{\δv}}_g={\mathrm{\ω}}_{\mathrm{ib}}^b\×r=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_z-{\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_y\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\·r_x-{\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_z\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·r_y-{\mathrm{\ω}}_{\mathrm{iby}}^b\·r_x\end{array}\right).$