CN103245359A - Method for calibrating fixed errors of inertial sensor in inertial navigation system in real time - Google Patents

Abstract

The invention discloses a method for calibrating fixed errors of an inertial sensor in an inertial navigation system in real time. The method comprises the following steps of: firstly, establishing a fixed error model of the inertial sensor during the dynamic flying process of an aircraft, wherein the fixed errors include installation errors and scale factor errors; secondly, establishing a state equation and a position, speed and attitude linear measurement equation of a filter, with the fixed errors of the inertial sensor included, on the basis of a random error model of a traditional IMU (Inertial Measurement Unit) and the established fixed error model; and finally, carrying out real-time dynamic calibration and correction to the fixed errors of the inertial sensor during the dynamic flying process of the aircraft, so as to obtain the navigation result of the inertial navigation system after the fixed errors of the inertial sensor are compensated and corrected. The method can realize the calibration and correction of installation errors and scale factor errors of the inertial sensor used in the inertial navigation system during the dynamic flying process of the aircraft, effectively enhances the performance of the inertial navigation system, and is suitable for engineering application.

Description

Inertial sensor fixed error real-time calibration method in a kind of inertial navigation system

Technical field

The invention discloses inertial sensor fixed error real-time calibration method in a kind of inertial navigation system, belong to inertial navigation inertial sensor error calibration technical field.

Background technology

In recent years, along with the development of course of new aircraft, the navigational system performance demands is improved day by day.Inertial navigation system has in short-term precision height, output continuously and outstanding advantage such as autonomous fully must be the core navigation information unit of following course of new aircraft.

The measuring error of inertial sensor (IMU-gyroscope and accelerometer) is to influence the inertial navigation system accuracy factors.In the conventional I MU error model, often think its solid error-alignment error and the full remuneration behind static demarcating of constant multiplier error only to comprise Random Drift Error in the imu error.And in the space vehicle dynamic flight course, the body deformation that is caused by influences such as vibratory impulse, flow perturbations will cause that the axis of IMU can not overlap fully with body axis, and then cause alignment error and constant multiplier error to enlarge markedly.If can not carrying out calibration compensation in the space vehicle dynamic flight course, these errors will influence navigation accuracy to a great extent.

Traditional imu error scaling method utilizes turntable to carry out position test and speed trial and realize demarcation to alignment error and constant multiplier error indoor more.But in the space vehicle dynamic flight course, because environmental impact, tradition utilizes the scaling method of turntable will be no longer suitable.In the space vehicle dynamic flight course, tradition only at its randomness error, is not considered the influence of solid errors such as its alignment error and constant multiplier error to the error correction of inertial sensor.And because alignment error and constant multiplier error that influences such as airborne vibration cause will influence the inertial navigation system precision to a great extent, must demarcate and compensate correction to it.Therefore, study a kind of in the space vehicle dynamic flight course scaling method to the inertial sensor solid error, can effectively improve the inertial navigation system precision in the space vehicle dynamic flight course, will have outstanding using value.

Summary of the invention

The objective of the invention is to propose a kind of solid error modeling and real-time calibration method of inertial navigation system inertial sensor, to satisfy the high-precision requirement to navigational system in the space vehicle dynamic flight course.

The present invention is for solving the problems of the technologies described above by the following technical solutions, inertial sensor fixed error real-time calibration method in a kind of inertial navigation system, and described method concrete steps are as follows:

Step 1, set up the solid error model of inertial sensor, comprise accelerometer constant multiplier error matrix and gyrostatic constant multiplier error matrix:

Accelerometer constant multiplier error matrix is δ K _A=diag[δ K _Axδ K _Ayδ K _Az], wherein, δ K _Ax, δ K _Ay, δ K _AzBe respectively the constant multiplier error of X-axis, Y-axis and Z axis accelerometer; Accelerometer alignment error matrix is $\mathrm{\&delta;A}=\left[\begin{array}{ccc}0& {\mathrm{\&delta;A}}_z& -{\mathrm{\&delta;A}}_y\\ -{\mathrm{\&delta;<figure-callout\; id="0"\; label="A\; z"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">A</figure-callout>}}_z& 0& {\mathrm{\&delta;A}}_x\\ {\mathrm{\&delta;A}}_y& -{\mathrm{\&delta;<figure-callout\; id="0"\; label="A\; x"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">A</figure-callout>}}_x& 0\end{array}\right],$ Wherein, δ A _x, δ A _y, δ A _zBe respectively the alignment error angle of X-axis, Y-axis and Z axis accelerometer; δ K _AAll be taken as arbitrary constant with δ A, the accelerometer error model that X, Y, Z are three is identical, as follows:

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_A=0\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{A}=0\end{array}\right.$

In the following formula,

Be constant multiplier error matrix δ K _AFirst order derivative, First order derivative for alignment error matrix delta A;

Gyrostatic constant multiplier error matrix is δ K _G=diag[δ K _Gxδ K _Gyδ K _Gz], δ K _Gx, δ K _Gy, δ K _GzBe respectively the constant multiplier error of X-axis, Y-axis and Z axle gyro; The gyro misalignment matrix is $\mathrm{\&delta;G}=\left[\begin{array}{ccc}0& {\mathrm{\&delta;G}}_z& -{\mathrm{\&delta;G}}_y\\ -{\mathrm{\&delta;<figure-callout\; id="0"\; label="G\; z"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">G</figure-callout>}}_z& 0& {\mathrm{\&delta;G}}_x\\ {\mathrm{\&delta;G}}_y& -{\mathrm{\&delta;G}}_x& 0\end{array}\right],$ δ G _x, δ G _y, δ G _zBe respectively the alignment error of X-axis, Y-axis and Z axle gyro; Constant multiplier sum of errors alignment error all is taken as arbitrary constant, and the gyro error model that X, Y, Z are three is identical, as follows:

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_G=0\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{G}=0\end{array}\right.$

In the following formula,

Be constant multiplier error matrix δ K _GFirst order derivative,

First order derivative for alignment error matrix delta G;

Step 2, on the basis of the alignment error of step 1 couple IMU and constant multiplier error modeling, the alignment error of IMU and constant multiplier error as system state variables, are made up filter status equation and position, speed and attitude linearization measurement equation based on inertial sensor total state error model:

System state variables X based on inertial sensor total state error modeling is:

X＝[φ _E φ _N φ _U δV _E δV _N δV _U δL δλ δh ε _bx ε _by ε _bz ε _rx ε _ry ε _rz ▽ _x ▽ _y ▽ _z δA _x δA _y δA _z δK _Ax δK _Ay δK _Az δG _x δG _y δG _z δK _Gx δK _Gy δK _Gz] ^T

In the following formula, φ _E, φ _N, φ _UBe respectively east orientation in the inertial navigation system, north orientation and day to platform error angle quantity of state; δ V _E, δ V _N, δ V _UBe respectively east orientation in the INS errors, north orientation and day to the velocity error quantity of state; δ L, δ λ, δ h are respectively latitude error quantity of state, longitude error quantity of state and the height error quantity of state in the inertial navigation system; ε _Bx, ε _By, ε _BzBe respectively the constant value drift error state amount of X-axis in the inertial navigation system, Y-axis, Z-direction gyro; ε _Rx, ε _Ry, ε _RzBe respectively the gyro single order Markov drift error quantity of state of X-axis, Y-axis and Z-direction; ▽ _x, ▽ _y, ▽ _zBe respectively the accelerometer single order Markov zero inclined to one side error state amount of X-axis, Y-axis and Z-direction; δ A _x, δ A _y, δ A _zBe respectively the alignment error quantity of state of accelerometer on X-axis in the inertial navigation system, Y-axis and the Z-direction; δ K _Ax, δ K _Ay, δ K _AzBe respectively the constant multiplier error state amount of accelerometer on X-axis in the inertial navigation system, Y-axis and the Z-direction; δ G _x, δ G _y, δ G _zBe respectively the alignment error quantity of state of gyro on X-axis in the inertial navigation system, Y-axis and three directions of Z axle; δ K _Gx, δ K _Gy, δ K _GzConstant multiplier error state amount for gyro on X-axis, Y-axis and the Z-direction in the inertial navigation system;

Set up the error state equation of inertial navigation system, as the formula (4):

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

In the following formula, X (t) is system state variables;

First order derivative for state variable X (t); F (t) is system matrix; G (t) is the noise figure matrix; W (t) is noise matrix;

Adopt Department of Geography's upper/lower positions, speed and attitude linearization observation principle, set up the linearization measurement equation between satellite navigation position detection amount, speed observed quantity and the observed quantity of star sensor attitude and INS errors quantity of state under the Department of Geography:

$Z\left(t\right)=\left[\begin{array}{c}{Z}_p\left(t\right)\\ Z_v\left(t\right)\\ Z_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{H}_p\left(t\right)\\ H_v\left(t\right)\\ H_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_p\left(t\right)\\ V_v\left(t\right)\\ V_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]$

In the following formula, Z _p(t), Z _v(t), Z _φ(t) be respectively position, speed and attitude and measure vector; H _p(t), H _v(t), H _φ(t) be respectively position, speed and attitude and measure matrix of coefficients; V _p(t), V _v(t), V _φ(t) be respectively position, speed and attitude observed quantity noise matrix;

Step 3, on the basis of step 2, system state equation and measurement equation are carried out the renewal of discretize processing and quantity of state, measurement amount, realize demarcation and compensation to IMU alignment error and constant multiplier error.

Further, to demarcation and the compensation of IMU alignment error and constant multiplier error, its concrete steps are described in the step 3:

(301) filter status equation and measurement equation discretize are handled:

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)

Z(k)＝H(k)X(k)+V(k)

In the following formula, X (k) is t _kMoment system state amount; X (k-1) is t _K-1Moment system state amount; (k k-1) is t to Φ _K-1Constantly to t _kThe time etching system state-transition matrix; (k k-1) is t to Γ _K-1Constantly to t _kThe time etching system noise drive matrix; W (k-1) is t _kThe time etching system noise matrix; Z (k) is t _kThe time etching system position, speed and attitude observed quantity matrix; H (k) is t _kPosition constantly, speed and attitude measure matrix of coefficients; V (k) is t _kThe noise matrix of position, speed and attitude observed quantity constantly;

(302) according to formula (8), formula (9), formula (10), formula (11) the INS errors quantity of state of step (301) part and the measurement of covariance information are upgraded:

P(k,k-1)＝Φ(k,k-1)P(k-1,k-1)Φ(k,k-1) ^T+Γ(k,k-1)Q(k)Γ(k,k-1) ^T

K(k)＝P(k,k-1)H(k) ^T[H(k)P(k,k-1)H(k) ^T+R(k)] ^-1

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)]

P(k,k)＝[I-K(k)H(k)]P(k,k-1)

In the above-mentioned formula, (k k-1) is t to P _K-1Constantly to t _kMoment one-step prediction covariance matrix; (k-1 k-1) is t to P _K-1Moment filter state estimate covariance matrix; Q (k) is t _kThe time etching system noise covariance matrix; Κ (k) is t _kMoment filter gain matrix; R (k) is t _kMoment position, speed and attitude measurement noise covariance matrix; (k k-1) is t to X _K-1Constantly to t _kMoment one-step prediction quantity of state; (k k) is t to P _kMoment filter state estimate covariance matrix; I is and P (k, k) the identical unit matrix of dimension;

(303) to the quantity of state X of filtering output (k k) judges, if quantity of state X (k, predicted value k) is stable then finishes demarcation, obtains the calibration result to IMU alignment error and constant multiplier error, enters step (304); If the predicted value instability is then returned step (302) and is continued state value is predicted estimation;

(304) after step (303) obtains calibration result to IMU alignment error and constant multiplier error, temporary calibration value, utilize calibration value that IMU alignment error and constant multiplier error are compensated correction, proofread and correct and finish in the cycle at a navigation calculation, the error compensation correcting algorithm is:

$\left\{\begin{array}{c}{f}^b=[I-{\mathrm{\&delta;K}}_A-\mathrm{\&delta;A}]({\tilde{f}}^b-{\&dtri;}^b)\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^b=[I-{\mathrm{\&delta;K}}_G-\mathrm{\&delta;G}]({\widetilde{\mathrm{\&omega;}}}_{\mathrm{ib}}^b-{\mathrm{\&epsiv;}}^b)\end{array}\right.$

In the following formula,

Be respectively the acceleration of accelerometer output of error and the angular speed of gyroscope output; f ^b,

Be respectively acceleration and the angular speed got rid of after the error; ▽ ^bBe the Random Drift Error of accelerometer, ε ^bRandom Drift Error for gyro.

The present invention is on the basis to the inertial sensor error modeling, can in the space vehicle dynamic flight course, realize the real-time calibration to the inertial sensor solid error, the result who utilizes demarcation to obtain can carry out real-Time Compensation to the inertial sensor error, adopt the present invention can significantly reduce INS errors and can effectively improve the inertial navigation system precision, be fit to engineering and use.

Description of drawings

Fig. 1 is inertial navigation system inertial sensor solid error real-time calibration method structural drawing of the present invention.

Fig. 2 is for demarcating used flight path.

Fig. 3 is the process alignment error calibration result of X-axis, Y-axis and Z axle gyro.

Fig. 4 is the constant multiplier error calibration result of X-axis, Y-axis and Z axle gyro.

Fig. 5 is the process alignment error calibration result of X-axis, Y-axis and Z axis accelerometer.

Fig. 6 is the constant multiplier error calibration result of X-axis, Y-axis and Z axis accelerometer.

Fig. 7 is the inertial navigation attitude error behind the inertial sensor solid error calibration compensation.

Fig. 8 is the inertial navigation velocity error behind the inertial sensor solid error calibration compensation.

Fig. 9 is the inertial navigation site error behind the inertial sensor solid error calibration compensation.

Embodiment

Below in conjunction with accompanying drawing technical scheme of the present invention is described in further detail: as shown in Figure 1, the principle of inertial sensor solid error real-time calibration method of the present invention is: the measurement output of gyro and accelerometer is respectively

And

Stationarity and randomness error have wherein all been comprised.

And f ^bIt is the navigation calculation module that is used for inertial navigation system through the revised angular speed of error compensation and acceleration information.Total state error modeling module is set up alignment error and the constant multiplier error model of IMU on the basis of traditional error model, make up filter state amount and the state equation of INS errors; Simultaneously, according to attitude, speed and position linearity observation principle under the Department of Geography, set up the linearization measurement equation of speed under the Department of Geography, position and attitude, realize the real-time calibration to inertial sensor alignment error and constant multiplier error; Utilize the result who demarcates to compensate and correct at the solid error of imu error correcting module to IMU, can effectively improve the inertial navigation system precision.

The specific embodiment of the present invention is as follows:

1. set up inertial sensor solid error model

(1.1) accelerometer error model

In strapdown inertial navigation system, because FLIGHT VEHICLE VIBRATION can cause gyroscope and accelerometer all to have alignment error and constant multiplier error.Ignore high-order nonlinear error term, the error model of accelerometer is:

δf ^b＝[δK _A+δA]f ^b+▽ ^b (1)

In the formula (1), δ K _A=diag[δ K _Axδ K _Ayδ K _Az] be the constant multiplier error of accelerometer;

▽=[▽ _x▽ _y▽ _z] ^TZero inclined to one side error for accelerometer; $\mathrm{\&delta;A}=\left[\begin{array}{ccc}0& {\mathrm{\&delta;A}}_z& -{\mathrm{\&delta;A}}_y\\ -{\mathrm{\&delta;A}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">z</figure-callout>}}& 0& {\mathrm{\&delta;A}}_x\\ {\mathrm{\&delta;A}}_y& -{\mathrm{\&delta;A}}_x& 0\end{array}\right]$ Alignment error angular moment battle array for accelerometer;

The constant multiplier error delta K of accelerometer _AAll be taken as arbitrary constant with alignment error δ A, zero inclined to one side error ▽ of accelerometer is taken as first-order Markov process: and suppose that the error model of three axis accelerometers is identical:

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_A=0\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{A}=0\\ \stackrel{\&CenterDot;}{\&dtri;}=-\frac{1}{T_a}\&dtri;+w_a\end{array}\right.---\left(2\right)$

(1.2) gyroscope error model

Similar with the accelerometer error modeling method, the error model of gyro is:

${\mathrm{\&delta;\&omega;}}_{\mathrm{ib}}^b=[{\mathrm{\&delta;K}}_G+\mathrm{\&delta;G}]{\mathrm{\&omega;}}_{\mathrm{ib}}^b+{\mathrm{\&epsiv;}}^b---\left(3\right)$

In the formula (3), δ K _G=diag[δ K _Gxδ K _Gyδ K _Gz] be the constant multiplier error matrix of gyro; $\mathrm{\&delta;G}=\left[\begin{array}{ccc}0& {\mathrm{\&delta;G}}_z& -{\mathrm{\&delta;G}}_y\\ -{\mathrm{\&delta;<figure-callout\; id="0"\; label="G\; z"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">G</figure-callout>}}_z& 0& {\mathrm{\&delta;G}}_x\\ {\mathrm{\&delta;G}}_y& -{\mathrm{\&delta;G}}_x& 0\end{array}\right]$ Alignment error angular moment battle array for gyro; Gyrostatic constant multiplier sum of errors

Alignment error all is taken as arbitrary constant, supposes that the error model of three axle gyros is identical, obtains:

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_G=0\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{G}=0\end{array}\right.---\left(4\right)$

Gyroscopic drift error ε _b=[ε _xε _yε _z] ^TBe taken as

ε＝ε _b+ε _r+w _g (5)

In the formula (5),

Be arbitrary constant,

Be first-order Markov process, w _gBe white noise.

2. set up the kalman filter models based on the total state error model

(2.1) set up the quantity of state equation of INS errors

Choose body axis system (b system) for " right, preceding, on " of body (X, Y, Z) direction, choosing navigation coordinate system (n system) is sky, northeast (ENU) geographic coordinate system.System state variables based on the total state error modeling is:

X＝[φ _E φ _N φ _U δV _E δV _N δV _U δL δλ δh ε _bx ε _by ε _bz ε _rx ε _ry ε _rz ▽ _x ▽ _y ▽ _z δA _x δA _y δA _z δK _Ax δK _Ay δK _Az δG _x δG _y δG _z δK _Gx δK _Gy δK _Gz] ^T

(6)

In the formula (6), φ _E, φ _N, φ _UBe respectively east orientation in the inertial navigation system, north orientation and day to platform error error angle quantity of state; δ V _E, δ V _N, δ V _UBe respectively east orientation in the INS errors, north orientation and day to the velocity error quantity of state; δ L, δ λ, δ h represent latitude error quantity of state, longitude error quantity of state and the height error quantity of state in the inertial navigation system; ε _Bx, ε _By, ε _BzBe respectively the constant value drift error state amount of X-axis in the inertial navigation system, Y-axis, Z-direction gyro; ε _Rx, ε _Ry, ε _RzBe respectively the gyro single order Markov drift error quantity of state of X-axis, Y-axis and Z-direction; ▽ _x, ▽ _y, ▽ _zBe respectively the accelerometer single order Markov zero inclined to one side error state amount of X-axis in the inertial navigation system, Y-axis and Z-direction; δ A _x, δ A _y, δ A _zAlignment error quantity of state for accelerometer on X-axis, Y-axis and the Z-direction in the inertial navigation system; δ K _Ax, δ K _Ay, δ K _AzConstant multiplier error state amount for accelerometer on X-axis, Y-axis and the Z-direction in the inertial navigation system; δ G _x, δ G _y, δ G _zAlignment error quantity of state for gyro on X-axis, Y-axis and three directions of Z axle in the inertial navigation system; δ K _Gx, δ K _Gy, δ K _GzConstant multiplier error state amount for gyro on X-axis, Y-axis and the Z-direction in the inertial navigation system;

Set up the error state equation of inertial navigation system, as the formula (7):

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(7\right)$

In the formula (7), X (t) is system state variables,

First order derivative for state variable X (t); F (t) is system matrix, and G (t) is the noise figure matrix, and W (t) is noise matrix.

According to the inertial sensor error model of setting up in the step 1, the system matrix F (t) corresponding with formula (6) and formula (7) is:

$F\left(t\right)=\left[\begin{array}{ccc}{\mathrm{FN}\left(t\right)}_{9\&times;9}& \mathrm{FS}{\left(t\right)}_{9\&times;9}& {\mathrm{Fs}\left(t\right)}_{9\&times;12}\\ 0_{9\&times;9}& {\mathrm{FM}\left(t\right)}_{9\&times;9}& 0_{9\&times;12}\\ & 0_{12\&times;30}& \end{array}\right]---\left(8\right)$

In the formula (8), FN (t) _{9 * 9}Be the relational matrix between corresponding 9 inertial navigation system navigational parameters (attitude, speed and position), its nonzero term element is:

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{1,2}\right)={\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L\\ \mathrm{FN}\left(\mathrm{1,3}\right)=-({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ \mathrm{FN}\left(\mathrm{1,5}\right)=-\frac{1}{R_M+h}\end{array}\right.$ $\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{2,1}\right)=-({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)\\ \mathrm{FN}\left(\mathrm{2,3}\right)=-\frac{V_N}{R_M+h}\\ \mathrm{FN}\left(\mathrm{2,4}\right)=\frac{1}{R_N+h}\\ \mathrm{FN}\left(\mathrm{2,7}\right)=-{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{3,1}\right)={\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}\\ \mathrm{FN}\left(\mathrm{3,2}\right)=\frac{V_N}{R_M+h}\\ \mathrm{FN}\left(\mathrm{3,4}\right)=\frac{\tan L}{R_N+h}\\ \mathrm{FN}\left(\mathrm{3,7}\right)={\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{4,2}\right)=-f_u\\ \mathrm{FN}\left(\mathrm{4,3}\right)=f_n\\ \mathrm{FN}\left(\mathrm{4,4}\right)=\frac{V_N\tan L-V_U}{R_N+h}\\ \mathrm{FN}\left(\mathrm{4,5}\right)={2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L\\ \mathrm{FN}\left(\mathrm{4,6}\right)=-(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ \mathrm{FN}\left(\mathrm{4,7}\right)={2\mathrm{\&omega;}}_{\mathrm{ie}}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{5,1}\right)=f_u\\ \mathrm{FN}\left(\mathrm{5,3}\right)=-f_e\\ \mathrm{FN}\left(\mathrm{5,4}\right)=-2({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)\\ \mathrm{FN}\left(\mathrm{5,5}\right)=-\frac{V_U}{R_M+h}\\ \mathrm{FN}\left(\mathrm{5,6}\right)=-\frac{V_N}{R_M+h}\\ \mathrm{FN}\left(\mathrm{5,7}\right)=-({2V}_E{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E^2}{R_N+h}{\sec }^{2}L)\end{array}\right.$ $\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{6,1}\right)=-f_n\\ \mathrm{FN}\left(\mathrm{6,2}\right)=f_e\\ \mathrm{FN}\left(\mathrm{6,4}\right)=2({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ \mathrm{FN}\left(\mathrm{6,5}\right)=\frac{{2V}_N}{R_M+h}\\ \mathrm{FN}\left(\mathrm{6,7}\right)=-{2V}_E{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{7,5}\right)=\frac{1}{R_M+h}\\ \mathrm{FN}\left(\mathrm{8,4}\right)=\frac{\sec L}{R_N+h}\\ \mathrm{FN}\left(\mathrm{8,7}\right)=\frac{V_E}{R}\\ \mathrm{FN}\left(\mathrm{9,6}\right)=1\end{array}\right.$

R _MBe earth radius of curvature of meridian, R _NBe earth radius of curvature in prime vertical; f _e, f _n, f _uFor east orientation, north orientation and day to acceleration.

In the formula (8), $\mathrm{FS}\left(t\right)=\left[\begin{array}{ccc}{C}_b^n& {\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& {\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}\\ & 0_{3\&times;9}& \end{array}\right],$

Be tied to the transition matrix of geographic coordinate system for body

${\mathrm{Fs}\left(t\right)}_{9\&times;12}=\left[\begin{array}{ccc}{0}_{3\&times;6}& F_1{\left(t\right)}_{3\&times;3}& F_3{\left(t\right)}_{3\&times;3}\\ F_2{\left(t\right)}_{3\&times;3}& F_4{\left(t\right)}_{3\&times;3}& 0_{3\&times;6}\\ & 0_{3\&times;12}& \end{array}\right]---\left(9\right)$

In the formula (9):

$F_1\left(t\right)=\left[\begin{array}{ccc}-C_{12}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b+C_{13}{\mathrm{\&omega;}}_{\mathrm{iby}}^b& C_{11}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b-C_{13}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -C_{11}{\mathrm{\&omega;}}_{\mathrm{iby}}^b+C_{12}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ -C_{22}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b+C_{23}{\mathrm{\&omega;}}_{\mathrm{iby}}^b& C_{21}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b-C_{23}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -C_{21}{\mathrm{\&omega;}}_{\mathrm{iby}}^b+C_{22}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ -C_{32}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b+C_{33}{\mathrm{\&omega;}}_{\mathrm{iby}}^b& C_{31}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b-C_{33}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -C_{31}{\mathrm{\&omega;}}_{\mathrm{iby}}^b+C_{32}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\end{array}\right],$

$F_2\left(t\right)=\left[\begin{array}{ccc}{C}_{12}{f}_z^b-C_{13}{f}_y^b& -C_{11}{f}_z^b+C_{13}{f}_x^b& C_{11}{f}_y^b-C_{12}{f}_x^b\\ C_{22}{f}_z^b-C_{23}{f}_y^b& -C_{21}{f}_z^b+C_{23}{f}_x^b& C_{21}{f}_y^b-C_{22}{f}_x^b\\ C_{32}{f}_z^b-C_{33}{f}_y^b& -C_{31}{f}_z^b+C_{33}{f}_x^b& C_{31}{f}_y^b-C_{32}{f}_x^b\end{array}\right]$

$F_3\left(t\right)=\left[\begin{array}{ccc}{-C}_{11}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -C_{12}{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -C_{13}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {-C}_{21}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -C_{22}{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -C_{23}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ -C_{31}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -C_{32}{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -C_{33}{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\end{array}\right],$ $F_4\left(t\right)=\left[\begin{array}{ccc}{C}_{11}{f}_x^b& C_{12}{f}_y^b& C_{13}{f}_z^b\\ C_{21}{f}_x^b& C_{22}{f}_y^b& C_{23}{f}_z^b\\ C_{31}{f}_x^b& C_{32}{f}_y^b& C_{33}{f}_z^b\end{array}\right]$

Wherein: 3 for body is tied to the transition matrix of geographic coordinate system, $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ Be the output of accelerometer in body system, ${\mathrm{\&omega;}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\end{array}\right]}^T$ Export the projection of fastening at body for gyro.

In the formula (8), the relational matrix between the inertial sensor error is:

$\mathrm{FM}\left(t\right)=\mathrm{<figure-callout\; id="0"\; label="Diag"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">Diag</figure-callout>}\left[\begin{array}{ccccccccc}0& 0& 0& -\frac{1}{T_{\mathrm{gx}}}& -\frac{1}{T_{\mathrm{gy}}}& -\frac{1}{T_{\mathrm{gz}}}& -\frac{1}{T_{\mathrm{ax}}}& -\frac{1}{T_{\mathrm{ay}}}& -\frac{1}{T_{\mathrm{az}}}\end{array}\right]---\left(10\right)$

The system noise matrix is:

W＝[w _gx w _gy w _gz w _rx w _ry w _rz w _ax w _ay w _az] (11)

The error coefficient matrix:

$G\left(t\right)=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="1304150908482.png,1304150908483.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{9\&times;3}& 0_{9\&times;3}& 0_{9\&times;3}\\ 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& I_{3\&times;3}\end{array}\right]---\left(12\right)$

(2.2) measurement equation determines

Adopt attitude, speed and position linearity observation principle under the Department of Geography, set up satellite navigation speed under the Department of Geography, position and the observed quantity of star sensor attitude and step 2.1) in linearization measurement equation between the INS errors quantity of state:

$Z\left(t\right)=\left[\begin{array}{c}{Z}_p\left(t\right)\\ Z_v\left(t\right)\\ Z_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{H}_p\left(t\right)\\ H_v\left(t\right)\\ H_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_p\left(t\right)\\ V_v\left(t\right)\\ V_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]---\left(13\right)$

In the formula, Z _p(t), Z _v(t) and Z _φ(t) be that position, speed and attitude measure vector.H _p(t), H _v(t), H _φ(t) be that position, speed and attitude measure matrix of coefficients, position, speed and attitude observed quantity noise matrix are V _p(t), V _v(t) and V _φ(t).3. the real-time calibration of inertial sensor solid error compensation

(3.1) filter status equation and measurement equation discretize are handled:

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1) (14)

Z(k)＝H(k)X(k)+V(k) (15)

In the formula (15), X (k) is t _kMoment system state amount; X (k-1) is t _K-1Moment system state amount; (k k-1) is t to Φ _K-1Constantly to t _kThe time etching system state-transition matrix; (k k-1) is t to Γ _K-1Constantly to t _kThe time etching system noise drive matrix; W (k-1) is t _kThe time etching system noise matrix; Z (k) is t _kThe time etching system position, speed and attitude observed quantity matrix; H (k) is t _kPosition constantly, speed and attitude measure matrix of coefficients; V (k) is t _kThe noise matrix of position, speed and attitude observed quantity constantly.

(3.2) according to formula (16), formula (17), formula (18), formula (19) to substep 3.1) ins error quantity of state and the measurement of covariance information of part upgrade:

P(k,k-1)＝Φ(k,k-1)P(k-1,k-1)Φ(k,k-1) ^T+Γ(k,k-1)Q(k)Γ(k,k-1) ^T

(16)

K(k)＝P(k,k-1)H(k) ^T[H(k)P(k,k-1)H(k) ^T+R(k)] ^-1 (17)

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)] (18)

P(k,k)＝[I-K(k)H(k)]P(k,k-1) (19)

Wherein, (k k-1) is t to P _K-1Constantly to t _kMoment one-step prediction covariance matrix; (k-1 k-1) is t to P _K-1Moment filter state estimate covariance matrix; Q (k) is t _kThe time etching system noise covariance matrix; Κ (k) is t _kMoment filter gain matrix; R (k) is t _kMoment position, speed and attitude measurement noise covariance matrix; (k k-1) is t to X _K-1Constantly to t _kMoment one-step prediction quantity of state; (k k) is t to P _kMoment filter state estimate covariance matrix;

I is and P (k, k) the identical unit matrix of dimension;

(3.3) (k k) judges the quantity of state X that filtering is exported, if the predicted value of quantity of state is stable, then finishes to demarcate entering step (3.4) if the predicted value instability is then returned step (3.2) continuation state value is predicted estimation.

(3.4) after step (3.3) obtains calibration result to IMU alignment error and constant multiplier error, temporary calibration value utilizes this calibration value that IMU alignment error and constant multiplier error are compensated correction, proofreaies and correct and finishes in the cycle at a navigation calculation.The error compensation correcting algorithm is:

$\left\{\begin{array}{c}{f}^b=[I-{\mathrm{\&delta;K}}_A-\mathrm{\&delta;A}]({\tilde{f}}^b-{\&dtri;}^b)\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^b=[I-{\mathrm{\&delta;K}}_G-\mathrm{\&delta;G}]({\widetilde{\mathrm{\&omega;}}}_{\mathrm{ib}}^b-{\mathrm{\&epsiv;}}^b)\end{array}\right.---\left(20\right)$

In the formula (20), δ K _A, δ A, ▽ ^bBe respectively constant multiplier error, the alignment error and partially zero of accelerometer, δ K _G, δ G, ε ^bFor constant multiplier error, the alignment error and partially zero at random of gyro, by obtaining in the calibration process.

(3.5) after step (3.4) is finished the alignment error of IMU and constant multiplier error correction compensation, do not re-use wave filter and enter pure-inertial guidance system works pattern.

For the solid error modeling of verifying the inertial navigation system inertial sensor that invention proposes and correctness and the validity of scaling method, adopt the inventive method to set up model, carry out the matlab simulating, verifying.The flight track that Fig. 2 adopted for when checking is to the calibration result of the alignment error of inertial sensor and constant multiplier error such as Fig. 3, Fig. 4, Fig. 5, shown in Figure 6.

In the space vehicle dynamic flight course, utilize calibration result that IMU alignment error and constant multiplier error are compensated and corrected, inertial navigation result after the compensation with compare checking without the inertial navigation navigation results of over-compensation, checking result such as Fig. 7, Fig. 8, shown in Figure 9.

Solid black lines represents calibration result of the present invention among Fig. 3, Fig. 4, Fig. 5, Fig. 6, and red dot-and-dash line is setting value.As can be seen from the figure to IMU alignment error and used time of constant multiplier error calibration about 20s, calibration result is consistent with setting value.The solid black lines representative does not compensate the pure inertial navigation navigation results of IMU solid error among Fig. 7, Fig. 8, Fig. 9, the pure inertial navigation navigation results behind the red dot-and-dash line representative compensation IMU solid error.From Fig. 7, Fig. 8, Fig. 9 as can be seen, utilize the calibration result of IMU alignment error and constant multiplier error proofreaied and correct the alignment error of IMU and constant multiplier error after, INS errors obviously reduces, and has useful engineering using value.

Claims (2)

1. inertial sensor fixed error real-time calibration method in the inertial navigation system is characterized in that described method concrete steps are as follows:

Step 1, set up the solid error model of inertial sensor, comprise accelerometer constant multiplier error matrix and gyrostatic constant multiplier error matrix:

Accelerometer constant multiplier error matrix is δ K _A=diag[δ K _Axδ K _Ayδ K _Az], wherein, δ K _Ax, δ K _Ay, δ K _AzBe respectively the constant multiplier error of X-axis, Y-axis and Z axis accelerometer; Accelerometer alignment error matrix is $\mathrm{\&delta;A}=\left[\begin{array}{ccc}0& {\mathrm{\&delta;A}}_z& -{\mathrm{\&delta;A}}_y\\ -{\mathrm{\&delta;A}}_z& 0& {\mathrm{\&delta;A}}_x\\ {\mathrm{\&delta;A}}_y& -{\mathrm{\&delta;A}}_x& 0\end{array}\right],$ Wherein, δ A _x, δ A _y, δ A _zBe respectively the alignment error angle of X-axis, Y-axis and Z axis accelerometer; δ K _AAll be taken as arbitrary constant with δ A, the accelerometer error model that X, Y, Z are three is identical, as follows:

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_A=0\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{A}=0\end{array}\right.$

In the following formula,

Be constant multiplier error matrix δ K _AFirst order derivative,

First order derivative for alignment error matrix delta A;

Gyrostatic constant multiplier error matrix is δ K _G=diag[δ K _Gxδ K _Gyδ K _Gz], δ K _Gx, δ K _Gy, δ K _GzBe respectively the constant multiplier error of X-axis, Y-axis and Z axle gyro; The gyro misalignment matrix is $\mathrm{\&delta;G}=\left[\begin{array}{ccc}0& {\mathrm{\&delta;G}}_z& -{\mathrm{\&delta;G}}_y\\ -{\mathrm{\&delta;G}}_z& 0& {\mathrm{\&delta;G}}_x\\ {\mathrm{\&delta;G}}_y& -{\mathrm{\&delta;G}}_x& 0\end{array}\right],$ δ G _x, δ G _y, δ G _zBe respectively the alignment error of X-axis, Y-axis and Z axle gyro; Constant multiplier sum of errors alignment error all is taken as arbitrary constant, and the gyro error model that X, Y, Z are three is identical, as follows:

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_G=0\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{G}=0\end{array}\right.$

In the following formula,

Be constant multiplier error matrix δ K _GFirst order derivative,

First order derivative for alignment error matrix delta G;

Step 2, on the basis of the alignment error of step 1 couple IMU and constant multiplier error modeling, the alignment error of IMU and constant multiplier error as system state variables, are made up filter status equation and position, speed and attitude linearization measurement equation based on inertial sensor total state error model:

System state variables X based on inertial sensor total state error modeling is:

X＝[φ _E φ _N φ _U δV _E δV _N δV _U δL δλ δh ε _bx ε _by ε _bz ε _rx ε _ry ε _rz ▽ _x ▽ _y ▽ _z δA _x δA _y δA _z δK _Ax δK _Ay δK _Az δG _x δG _y δG _z δK _Gx δK _Gy δK _Gz] ^T

In the following formula, φ _E, φ _N, φ _UBe respectively east orientation in the inertial navigation system, north orientation and day to platform error angle quantity of state; δ V _E, δ V _N, δ V _UBe respectively east orientation in the INS errors, north orientation and day to the velocity error quantity of state; δ L, δ λ, δ h are respectively latitude error quantity of state, longitude error quantity of state and the height error quantity of state in the inertial navigation system; ε _Bx, ε _By, ε _BzBe respectively the constant value drift error state amount of X-axis in the inertial navigation system, Y-axis, Z-direction gyro; ε _Rx, ε _Ry, ε _RzBe respectively the gyro single order Markov drift error quantity of state of X-axis, Y-axis and Z-direction; ▽ _x, ▽ _y, ▽ _zBe respectively the accelerometer single order Markov zero inclined to one side error state amount of X-axis, Y-axis and Z-direction; δ A _x, δ A _y, δ A _zBe respectively the alignment error quantity of state of accelerometer on X-axis in the inertial navigation system, Y-axis and the Z-direction; δ K _Ax, δ K _Ay, δ K _AzBe respectively the constant multiplier error state amount of accelerometer on X-axis in the inertial navigation system, Y-axis and the Z-direction; δ G _x, δ G _y, δ G _zBe respectively the alignment error quantity of state of gyro on X-axis in the inertial navigation system, Y-axis and three directions of Z axle; δ K _Gx, δ K _Gy, δ K _GzConstant multiplier error state amount for gyro on X-axis, Y-axis and the Z-direction in the inertial navigation system;

Set up the error state equation of inertial navigation system, as the formula (4):

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

In the following formula, X (t) is system state variables;

First order derivative for state variable X (t); F (t) is system matrix; G (t) is the noise figure matrix; W (t) is noise matrix;

Adopt Department of Geography's upper/lower positions, speed and attitude linearization observation principle, set up the linearization measurement equation between satellite navigation position detection amount, speed observed quantity and the observed quantity of star sensor attitude and INS errors quantity of state under the Department of Geography:

$Z\left(t\right)=\left[\begin{array}{c}{Z}_p\left(t\right)\\ Z_v\left(t\right)\\ Z_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{H}_p\left(t\right)\\ H_v\left(t\right)\\ H_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_p\left(t\right)\\ V_v\left(t\right)\\ V_{\mathrm{\&phi;}}\left(t\right)\end{array}\right]$

In the following formula, Z _p(t), Z _v(t), Z _φ(t) be respectively position, speed and attitude and measure vector; H _p(t), H _v(t), H _φ(t) be respectively position, speed and attitude and measure matrix of coefficients; V _p(t), V _v(t), V _φ(t) be respectively position, speed and attitude observed quantity noise matrix;

Step 3, on the basis of step 2, system state equation and measurement equation are carried out the renewal of discretize processing and quantity of state, measurement amount, realize demarcation and compensation to IMU alignment error and constant multiplier error.

2. inertial sensor fixed error real-time calibration method in a kind of inertial navigation system as claimed in claim 1 is characterized in that to demarcation and the compensation of IMU alignment error and constant multiplier error, its concrete steps are described in the step 3:

(301) filter status equation and measurement equation discretize are handled:

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)

Z(k)＝H(k)X(k)+V(k)

In the following formula, X (k) is t _kMoment system state amount; X (k-1) is t _K-1Moment system state amount; (k k-1) is t to Φ _K-1Constantly to t _kThe time etching system state-transition matrix; (k k-1) is t to Γ _K-1Constantly to t _kThe time etching system noise drive matrix; W (k-1) is t _kThe time etching system noise matrix; Z (k) is t _kThe time etching system position, speed and attitude observed quantity matrix; H (k) is t _kPosition constantly, speed and attitude measure matrix of coefficients; V (k) is t _kThe noise matrix of position, speed and attitude observed quantity constantly;

(302) according to formula (8), formula (9), formula (10), formula (11) the INS errors quantity of state of step (301) part and the measurement of covariance information are upgraded:

P(k,k-1)＝Φ(k,k-1)P(k-1,k-1)Φ(k,k-1) ^T+Γ(k,k-1)Q(k)Γ(k,k-1) ^T

K(k)＝P(k,k-1)H(k) ^T[H(k)P(k,k-1)H(k) ^T+R(k)] ^-1

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)]

P(k,k)＝[I-K(k)H(k)]P(k,k-1)

In the above-mentioned formula, (k k-1) is t to P _K-1Constantly to t _kMoment one-step prediction covariance matrix; (k-1 k-1) is t to P _K-1Moment filter state estimate covariance matrix; Q (k) is t _kThe time etching system noise covariance matrix; Κ (k) is t _kMoment filter gain matrix; R (k) is t _kMoment position, speed and attitude measurement noise covariance matrix; (k k-1) is t to X _K-1Constantly to t _kMoment one-step prediction quantity of state; (k k) is t to P _kMoment filter state estimate covariance matrix; I is and P (k, k) the identical unit matrix of dimension;

(303) to the quantity of state X of filtering output (k k) judges, if quantity of state X (k, predicted value k) is stable then finishes demarcation, obtains the calibration result to IMU alignment error and constant multiplier error, enters step (304); If the predicted value instability is then returned step (302) and is continued state value is predicted estimation;

(304) after step (303) obtains calibration result to IMU alignment error and constant multiplier error, temporary calibration value, utilize calibration value that IMU alignment error and constant multiplier error are compensated correction, proofread and correct and finish in the cycle at a navigation calculation, the error compensation correcting algorithm is:

$\left\{\begin{array}{c}{f}^b=[I-{\mathrm{\&delta;K}}_A-\mathrm{\&delta;A}]({\tilde{f}}^b-{\&dtri;}^b)\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^b=[I-{\mathrm{\&delta;K}}_G-\mathrm{\&delta;G}]({\widetilde{\mathrm{\&omega;}}}_{\mathrm{ib}}^b-{\mathrm{\&epsiv;}}^b)\end{array}\right.$

In the following formula,

Be respectively the acceleration of accelerometer output of error and the angular speed of gyroscope output; f ^b,

Be respectively acceleration and the angular speed got rid of after the error; ▽ ^bBe the Random Drift Error of accelerometer, ε ^bRandom Drift Error for gyro.

Images