CN102721424B - Method for multi-position optimal estimation detection of key parameters of incomplete-freedom degree inertial platform - Google Patents

Abstract

The invention relates to a method for multi-position optimal estimation detection of key parameters of an incomplete-freedom degree inertial platform. The method realizes on-line self-detection of an incomplete-freedom degree inertial platform, is suitable for self-detection of various types of incomplete and complete freedom degree inertial platforms and belongs to the field of on-line self-detection of parameters. A basic principle of the method comprises that under preconditions of no use of other applied equipment and no disassembling of a platform system, through outputs of an accelerometer and a gyroscope at the time when the gesture of the platform system changes, the combination with output models of the accelerometer and the gyroscope, and utilization of an extended Kalman filter method, key parameters of the platform system are detected in real time. The method realizes real-time self-detection and feedback of key parameters of a platform system, is conducive to timely compensation of a platform system and improves work precision and stability of a platform system. The method realizes real-time self-detection and feedback of key parameters of a platform system, also realizes real-time estimation of an error angle of the platform system, and improves calculation precision of other parameters.

Description

Non-fully free degree inertial platform key parameter multiposition optimal estimation detection method

Technical field

The present invention relates to a kind of On-line self-diagnosis survey method realizing non-fully free degree inertial platform, be applicable to the Inertial Platform System Autonomous test of all kinds of non-fully and complete degree of freedom, belong to the online autonomous detection field of parameter.

Background technology

Non-fully free degree Inertial Platform System is widely used on guided missile, aircraft, spacecraft, has been the important leverage of set objective.Inertial Platform System, as " heart " of aircraft navigation and guidance, improves the certainty of measurement of Inertial Platform System, directly effectively can improve the fighting efficiency of aircraft.

Some of non-fully free degree Inertial Platform System key parameters such as various drifts, accelerometer bias as gyro are detected and be improve the very effective means of inertia system service precision to error adequate compensation, by research non-fully free degree Inertial Platform System high-precision detecting method, can more drift about and improve Inertial Platform System service precision by effective compensation Systematic Error in Inertial Platform.

Current existing detection method: non-fully free degree Inertial Platform System pulled down from carrier, is placed on the detection carrying out key parameter in high-precision detection of platform system.Although this method can obtain high-precision testing result, but in actual applications, consider the factor of the aspect such as structure and installing space of carrier, the installation of non-fully free degree Inertial Platform System often complexity very, therefore the dismounting of this plateform system is very inconvenient.Therefore, this method is unfavorable for practical operation.

Summary of the invention

The object of the invention is the deficiency existed to overcome prior art, a kind of online autonomous method detected is provided.The general principle of this programme is: not by other equipment additional and do not pull down plateform system prerequisite under, platform is utilized to be in the output of different attitude brief acceleration meter and gyro and in conjunction with the output model of accelerometer and gyro, to use EKF method to carry out real-time Autonomous test to the key parameter of platform.

The object of the invention is to be achieved through the following technical solutions.

A kind of non-fully free degree inertial platform key parameter multiposition optimal estimation detection method, comprises the steps:

Step one, the acceleration of gravity in sky, northeast geographic coordinate system and rotational-angular velocity of the earth to be transformed in platform coordinate system.

Ground velocity is expressed as in geographic coordinate system

Ground velocity is expressed as in platform coordinate system

Wherein, n represents geographic coordinate system, and p represents platform coordinate system.α, beta, gamma is respectively platform around X, Y, the corner of Z axis, M ₁(α), M ₂(β), M ₃(γ) for platform is around X, Y, the transfer matrix that Z axis rotates.

Rotational-angular velocity of the earth ω _ebe expressed as in geographic coordinate system

${\mathrm{\ω}}_e=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ex}}\\ {\mathrm{\ω}}_{\mathrm{ey}}\\ {\mathrm{\ω}}_{\mathrm{ez}}\end{array}\right]$

Wherein, ω _ex=0, for geographic latitude.

Step 2, after completing ordinate transform, set up the Kalman filter state equation of non-fully free degree inertial platform according to the dynamical equation of error angle, gyro drift error model; Get acceleration and take into account angular transducer output as observed quantity and in conjunction with accelerometer linear convergent rate model, the observational equation of derivation Kalman filter.

Error angle dynamical equation is as follows

Wherein, φ _x, φ _y, φ _zbe respectively platform around X-axis, the error angle of Y-axis and Z axis.ε _x, ε _y, ε _zrepresent X gyro respectively, the drift of Y gyro and Z gyro.Its concrete form is as follows

X gyroscopic drift error model is

Y gyroscopic drift error model is

Z gyroscopic drift error model is

In upper three formulas, ε _gx, ε _gy, ε _gzbe respectively X gyro, the random drift of Y gyro and Z gyro.K _g0ifor gyroscope drift, k _g11ifor the gyro input axis Monomial coefficient of correspondence, k _g12ifor the gyroscope axis of rotation Monomial coefficient of correspondence, k _g13ifor the gyro output axis Monomial coefficient of correspondence, (i represents x or y or z).

Therefore non-fully free degree Inertial Platform System state equation is obtained as follows

Wherein $X={\left[\begin{array}{ccc}{\mathrm{\φ}}^T& k_g^T& k_a^T\end{array}\right]}^T,$ φ＝[φ _xφ _yφ _z] ^T，

k _g＝[k _g0xk _g0yk _g0zk _g11xk _g11yk _g11zk _g12xk _g12yk _g12zk _g13xk _g13yk _g13z] ^T

k _a＝[k _a0xk _a0yk _a0zk _a1xk _a1yk _a1zθ _pxθ _pyθ _pzθ _oxθ _oyθ _oz] ^T

Wherein k _a0iinclined for adding table zero, k _a1ifor adding table scale factor, θ _pi, θ _oifor adding the alignment error shown around balance staff and output shaft.W is white Gaussian noise interference vector.

$A=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ez}}& -{\mathrm{\ω}}_{\mathrm{ey}}\\ -{\mathrm{\ω}}_{\mathrm{ez}}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ey}}& 0& 0\end{array}\right],$

Accelerometer linear convergent rate model is:

Platform X accelerometer linear convergent rate model

Platform Y accelerometer linear convergent rate model

Platform Z accelerometer linear convergent rate model

E _aifor adding table random error.

Then observational equation is

Wherein

$Z=\left[\begin{array}{c}{Z}_a\\ Z_{\mathrm{an}}\end{array}\right],$

Z _afor three axis accelerometer exports, Z _anbe that three axle angular transducers export, φ ₀=[α ₀β ₀γ ₀] ^tfor the corner that diverse location is corresponding, v is 6 dimension white Gaussian noise interference vectors.

Step 3, use the observability matrix of Lie derivatives method to the non-fully free degree Inertial Platform System that step 2 obtains to calculate according to selected location, judge that whether system is completely considerable, namely whether selected location scheme is feasible.

The Lie Derivative Definition of nonlinear system is

$L_f^{k}h\left(X\right)=\frac{{\∂L}_f^{k-1}h\left(X\right)}{\∂X}f\left(X\right),L_f^{0}h\left(X\right)=h\left(X\right)---\left(12\right)$

Wherein k is differential times.

Defining vectorial L is

$L=\left[\begin{array}{c}h\left(X\right)\\ L_f^{1}h\left(X\right)\\ M\\ L_f^{n-1}h\left(X\right)\end{array}\right]=\left[\begin{array}{c}{L}_0\\ L_1\\ M\\ L_{n-1}\end{array}\right]---\left(13\right)$

Wherein n is the dimension of vectorial X, then the Jacobi matrix of L to X is

$M=\frac{\∂L}{\∂X}=\left[\begin{array}{ccc}\frac{{\∂L}_0}{{\∂X}_1}& L& \frac{{\∂L}_0}{{\∂X}_n}\\ M& & M\\ \frac{{\∂L}_{n-1}}{{\∂X}_1}& L& \frac{{\∂L}_{n-1}}{{\∂X}_1}\end{array}\right]---\left(14\right)$

Detect for multiposition, the matrix that (14) formula of being brought into by each position design parameter obtains is designated as M _i, then the observability matrix of system is

P＝[M ₁M ₂L M _k].

If the order of P is less than n, again calculate observability matrix P after chosen position; If its order is n, then system is completely considerable, and then can detect whole error coefficient, namely carries out step 4.

Step 4, non-fully free degree Inertial Platform System is gone to the position chosen, export Jia Biao and angular transducer and carry out continuous acquisition, utilize step 2, the EKF filter system equation that step 3 obtains detects platform error angle and each error coefficient.

Beneficial effect

With existing non-fully free degree Inertial Platform System Comparison between detecting methods, the inventive method reduces the inconvenience of disassembly system greatly, and utilize this method to carry out real-time Autonomous test and feedback to the key parameter of system, be conducive to compensating timely system, improve the precision and stability of the work of plateform system.In addition, in parameter calculation process, traditional calculation method ignores the Eulerian angles α between initial platform coordinate system and geographic coordinate system usually ₀, β ₀, γ ₀.The calculation accuracy of other key parameters will be affected like this.The inventive method, while carrying out real-time Autonomous test and feedback to the key parameter of system, can realize, to the real-time estimation of plateform system error angle, improve the calculation accuracy of other parameters.

Because in this method, parameter calculation method mainly uses the method for EKF, do not require that system equation and observational equation are linear, namely remain the high-order a small amount of in original equation, only at filtering stage, first approximation is carried out to equation.Therefore, not bery strict to requirement of real-time, and in the higher system of precision, adopt the inventive method can carry out the autonomous detection of platform key parameter more exactly.

Accompanying drawing explanation

Fig. 1 is the location schemes schematic diagram that in specific embodiment, platform independently detects;

Fig. 2 is that error angle true value in specific embodiment and detected value compare schematic diagram, and wherein a, b, c are respectively the error angle true value of platform X-axis, Y-axis and Z axis and detected value compares schematic diagram;

Fig. 3 is that gyroscope drift true value in specific embodiment and detected value compare schematic diagram, and wherein a, b, c are respectively the gyrostatic drift true value of X-axis, Y-axis and Z axis and detected value compares schematic diagram;

Fig. 4 is that gyroscope sensitive axes Monomial coefficient true value in specific embodiment and detected value compare schematic diagram, and wherein a, b, c are respectively X-axis, Y-axis and Z axis gyrostatic sensitive axes Monomial coefficient true value and detected value compares schematic diagram;

Fig. 5 is that gyroscope axis of rotation Monomial coefficient true value in specific embodiment and detected value compare schematic diagram, and wherein a, b, c are respectively X-axis, Y-axis and Z axis gyrostatic axis of rotation Monomial coefficient true value and detected value compares schematic diagram;

Fig. 6 is that gyro output axis Monomial coefficient true value in specific embodiment and detected value compare schematic diagram, and wherein a, b, c are respectively X-axis, Y-axis and Z axis gyrostatic output shaft Monomial coefficient true value and detected value compares schematic diagram;

Fig. 7 be in specific embodiment add table zero partially true value and detected value compare schematic diagram, wherein a, b, c are respectively X-axis, Y-axis and Z axis and add zero inclined true value of table and detected value compares schematic diagram;

Fig. 8 be in specific embodiment adding table scale factor true value and detected value compares schematic diagram, wherein a, b, c are respectively X-axis, Y-axis and Z axis and add the scale factor true value of table and detected value compares schematic diagram;

Fig. 9 is that adding table alignment error true value and the detected value in specific embodiment compares schematic diagram wherein a, b, c are respectively X-axis, Y-axis and Z axis and add table around the alignment error of balance staff; D, e, f are respectively X-axis, Y-axis and Z axis and add the alignment error of table around output shaft.

Detailed description of the invention

In order to better objects and advantages of the present invention are described, below in conjunction with drawings and Examples, the present invention will be further described.

In the present embodiment, carry out the Autonomous test of key parameter in conjunction with actual Inertial Platform System.In plateform system, the random drift of three accelerometers is 1 × 10 ^-6m/s ², three gyrostatic random drifts are 0.0001 °/h.Its process is as follows:

Step one, provide acceleration of gravity and the expression of rotational-angular velocity of the earth in Department of Geography, derivation geographical coordinate is tied to the transformational relation of platform coordinate system, and then obtains the expression of acceleration of gravity in platform system.

Ground velocity is expressed as in geographic coordinate system

Ground velocity is expressed as in platform system

Wherein, α, beta, gamma is respectively platform around X, Y, the corner of Z axis, M ₁(α), M ₂(β), M ₃(γ) be around X, Y, the transfer matrix that Z axis rotates.

Rotational-angular velocity of the earth is expressed as in Department of Geography

${\mathrm{\ω}}_e=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ex}}\\ {\mathrm{\ω}}_{\mathrm{ey}}\\ {\mathrm{\ω}}_{\mathrm{ez}}\end{array}\right]$

Wherein, ω _ex=0, for geographic latitude.

Step 2, dynamical equation according to error angle, gyro drift error model and accelerometer output equation set up Kalman filter system equation.

Error angle dynamical equation is as follows

Wherein, φ _x, φ _y, φ _zbe respectively platform around X-axis, the error angle of Y-axis and Z axis.ε _x, ε _y, ε _zrepresent X gyro respectively, the drift of Y gyro and Z gyro, its concrete form is as follows

X gyroscopic drift error model is

Y gyroscopic drift error model is

Z gyroscopic drift error model is

In upper three formulas, ε _gx, ε _gy, ε _gzbe respectively X gyro, the random drift of Y gyro and Z gyro.System state equation is obtained as follows according to above derivation

Wherein $X={\left[\begin{array}{ccc}{\mathrm{\φ}}^T& k_g^T& k_a^T\end{array}\right]}^T,$ φ＝[φ _xφ _yφ _z] ^T，

k _g＝[k _g0xk _g0yk _g0zk _g11xk _g11yk _g11zk _g12xk _g12yk _g12zk _g13xk _g13yk _g13z] ^T

k _a＝[k _a0xk _a0yk _a0zk _a1xk _a1yk _a1zθ _pxθ _pyθ _pzθ _oxθ _oyθ _oz] ^T，

W is 27 dimension white Gaussian noise interference vectors,

$A=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ez}}& -{\mathrm{\ω}}_{\mathrm{ey}}\\ -{\mathrm{\ω}}_{\mathrm{ez}}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ey}}& 0& 0\end{array}\right],$

Get acceleration and take into account angular transducer output as observed quantity

Accelerometer linear convergent rate model is:

Platform X accelerometer linear convergent rate model

Platform Y accelerometer linear convergent rate model

Platform Z accelerometer linear convergent rate model

E _aifor adding table random error

Then observational equation is

Wherein

$Z=\left[\begin{array}{c}{Z}_a\\ Z_{\mathrm{an}}\end{array}\right],$

Z _afor three axis accelerometer exports, Z _anbe that three axle angular transducers export, φ ₀=[α ₀β ₀γ ₀] ^tfor the corner that diverse location is corresponding, v is 6 dimension white Gaussian noise interference vectors.

Step 3, utilization Lie derivatives method carry out Analysis on Observability to this system

The Lie Derivative Definition of nonlinear system is

$L_f^{k}h\left(X\right)=\frac{{\∂L}_f^{k-1}h\left(X\right)}{\∂X}f\left(X\right),L_f^{0}h\left(X\right)=h\left(X\right)---\left(26\right)$

Wherein k is differential times.

Defining vectorial L is

$L=\left[\begin{array}{c}h\left(X\right)\\ L_f^{1}h\left(X\right)\\ M\\ L_f^{n-1}h\left(X\right)\end{array}\right]=\left[\begin{array}{c}{L}_0\\ L_1\\ M\\ L_{n-1}\end{array}\right]---\left(27\right)$

Wherein n is the dimension of vectorial X, then the Jacobi matrix of L to X is

$M=\frac{\∂L}{\∂X}=\left[\begin{array}{ccc}\frac{{\∂L}_0}{{\∂X}_1}& L& \frac{{\∂L}_0}{{\∂X}_n}\\ M& & M\\ \frac{{\∂L}_{n-1}}{{\∂X}_1}& L& \frac{{\∂L}_{n-1}}{{\∂X}_1}\end{array}\right]---\left(28\right)$

The matrix that (28) formula of being brought into by each position design parameter obtains is designated as M _i, the present embodiment chosen position is as shown in table 1, then the observability matrix of system is P=[M ₁m ₂l M ₁₅].Its order is 27 as calculated, and then can detect whole error coefficient.

Step 4, according to EKF filter equation, platform error angle and each error coefficient to be detected.15 positions are got in the present embodiment.

Table 1. location schemes

Platform stops 5 minutes in each position, and take into account angular transducer output to acceleration and carry out continuous acquisition, the sampling interval is taken as T=1s, and the data according to gathering carry out EKF filter estimation to quantity of state, and EKF equation is as follows:

$\left\{\begin{array}{c}\hat{X}(k,k-1)=\hat{X}(k-1)+f\left(\hat{X}(k-1)\right)T\\ P(k,k-1)=\mathrm{\Φ}(k,k-1)P(k-1){\mathrm{\Φ}}^T(k,k-1)+Q(k-1)\\ K\left(k\right)=P(k,k-1)H^T\left(k\right){[H\left(k\right)P(k,k-1)H^T\left(k\right)+R\left(k\right)]}^{-1}\\ P\left(k\right)=[I-K\left(k\right)H\left(k\right)]P(k,k-1){[I-K\left(k\right)H\left(k\right)]}^T+K\left(k\right)R\left(k\right)K^T\left(k\right)\\ \hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[z\left(k\right)-h\left(\hat{X}(k,k-1)\right)]\end{array}\right.---\left(19\right)$

In formula, for the state-transition matrix of system; for the observing matrix of system; The variance matrix that Q (k-1) is system model error w; R (k) is the variance matrix of systematic observation error v.

The initial estimated state of EKF can be taken as X ₀=zeros (27,1), initial variance battle array can be taken as P ₀(1:3,1:3)=1e-8*eye (3), P ₀(4:27,4:27)=1e-10*eye (24), system model error covariance matrix Q (k-1) can choose according to the random drift of actual gyro, and systematic observation error covariance matrix R (k) can be chosen according to actual acceleration meter random error and angular transducer random error.Choosing method is as follows:

Think the random drift of gyro, accelerometer random error and angular transducer random error, then have

Q(k-1)＝1/12*(2ε _g) ²*eye(3)，

$R\left(k\right)=\left[\begin{array}{cc}1/12*{\left({2e}_a\right)}^2*\mathrm{eye}\left(3\right)& 0\\ 0& 1/12*{\left({2\mathrm{\δ}}_a\right)}^2*\mathrm{eye}\left(3\right)\end{array}\right].$

In order to effect of the present invention is described, the true value of the error angle obtained in test and each error coefficient and detected value comparison diagram are as shown in Fig. 2 ~ Fig. 9.Fig. 2 be X, Y, Z tri-axle error angle true value and the comparison diagram of detected value, its abscissa is the time, and ordinate is X, Y, Z tri-error angle numerical value of axle; Fig. 3 is true value and the detected value comparison diagram of three-axis gyroscope drift, and its abscissa is the time, and ordinate is the drift numerical value of three-axis gyroscope; Fig. 4 is true value and the detected value comparison diagram of three-axis gyroscope power shaft Monomial coefficient, and its abscissa is the time, and ordinate is the power shaft Monomial coefficient numerical value of three-axis gyroscope; Fig. 5 is true value and the detected value comparison diagram of three-axis gyroscope axis of rotation Monomial coefficient, and its abscissa is the time, and ordinate is the axis of rotation Monomial coefficient numerical value of three-axis gyroscope; Fig. 6 is true value and the detected value comparison diagram of three-axis gyroscope output shaft Monomial coefficient, and its abscissa is the time, and ordinate is the output shaft Monomial coefficient numerical value of three-axis gyroscope; Fig. 7 is true value and the detected value comparison diagram of three axis accelerometer drift, and its abscissa is the time, and ordinate is three axis accelerometer drift numerical value; Fig. 8 is true value and the detected value comparison diagram of three axis accelerometer scale factor, and its abscissa is the time, and ordinate is three axis accelerometer scale factor numerical value; Fig. 9 is true value and the detected value comparison diagram of three axis accelerometer alignment error, and its abscissa is the time, and ordinate is three axis accelerometer alignment error numerical value.From result of the test figure, adopt the present invention can realize three error angles of three-axis platform and more accurately estimating of each key parameter, wherein, to error angle, gyroscope error coefficients and add table alignment error estimated accuracy higher, and convergence rate is very fast, and it is slightly low to add table zero estimated accuracy that is inclined and scale factor, and convergence time is longer, this is by adding zero of table partially and the observability degree of scale factor is lower causes, thus in the process of chosen position should with can to add table zero inclined and scale factor obtain better be estimated as good.

The above is only the preferred embodiment of the present invention; should be understood that; for those skilled in the art; under the premise without departing from the principles of the invention; some improvement can also be made; or carry out equivalent replacement to wherein portion of techniques feature, these improve and replace and also should be considered as protection scope of the present invention.

Claims (1)

1. non-fully free degree inertial platform key parameter multiposition optimal estimation detection method, is characterized in that: comprise the steps:

Step one, the acceleration of gravity in sky, northeast geographic coordinate system and rotational-angular velocity of the earth are transformed in platform coordinate system;

Ground velocity is expressed as in geographic coordinate system

${\stackrel{.}{W}}_n=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]---\left(1\right)$

Ground velocity is expressed as in platform coordinate system

${\stackrel{.}{W}}_p=M_3\left(\mathrm{\γ}\right)M_2\left(\mathrm{\β}\right)M_1\left(\mathrm{\α}\right){\stackrel{.}{W}}_n=\left[\begin{array}{c}{\stackrel{.}{W}}_{\mathrm{px}}\\ {\stackrel{.}{W}}_{\mathrm{py}}\\ {\stackrel{.}{W}}_{\mathrm{pz}}\end{array}\right]=\left[\begin{array}{c}-g(-\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\α})\\ -g(\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\α})\\ -g\cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]---\left(2\right)$

Wherein, n represents geographic coordinate system, and p represents platform coordinate system; α, beta, gamma is respectively platform around X, Y, the corner of Z axis, M ₁(α), M ₂(β), M ₃(γ) for platform is around X, Y, the transfer matrix that Z axis rotates;

Rotational-angular velocity of the earth ω _ebe expressed as in geographic coordinate system

${\mathrm{\ω}}_e=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ex}}\\ {\mathrm{\ω}}_{\mathrm{ey}}\\ {\mathrm{\ω}}_{\mathrm{ez}}\end{array}\right]$

Wherein, ω _ex=0, for geographic latitude;

Step 2, after completing ordinate transform, set up the Kalman filter state equation of non-fully free degree inertial platform according to the dynamical equation of error angle, gyro drift error model; Get acceleration and take into account angular transducer output as observed quantity and in conjunction with accelerometer linear convergent rate model, the observational equation of derivation Kalman filter;

Error angle dynamical equation is as follows

$\begin{array}{c}{\stackrel{.}{\mathrm{\φ}}}_x={\mathrm{\ω}}_{\mathrm{ez}}{\mathrm{\φ}}_y-{\mathrm{\ω}}_{\mathrm{ey}}{\mathrm{\φ}}_z+{\mathrm{\ϵ}}_x\\ {\stackrel{.}{\mathrm{\φ}}}_y=-{\mathrm{\ω}}_{\mathrm{ez}}{\mathrm{\φ}}_x+{\mathrm{\ϵ}}_y\\ {\stackrel{.}{\mathrm{\φ}}}_z={\mathrm{\ω}}_{\mathrm{ey}}{\mathrm{\φ}}_x+{\mathrm{\ϵ}}_z\end{array}---\left(3\right)$

Wherein, φ _x, φ _y, φ _zbe respectively platform around X-axis, the error angle of Y-axis and Z axis; ε _x, ε _y, ε _zrepresent X gyro respectively, the drift of Y gyro and Z gyro; Its concrete form is as follows

X gyroscopic drift error model is

${\mathrm{\ϵ}}_x=k_{g0x}+k_{g11x}{\stackrel{.}{W}}_{\mathrm{px}}+k_{g12x}{\stackrel{.}{W}}_{\mathrm{py}}+k_{g13x}{\stackrel{.}{W}}_{\mathrm{pz}}+{\mathrm{\ϵ}}_{\mathrm{gx}}---\left(4\right)$

Y gyroscopic drift error model is

${\mathrm{\ϵ}}_y=k_{g0y}+k_{g11y}{\stackrel{.}{W}}_{\mathrm{py}}+k_{g12y}{\stackrel{.}{W}}_{\mathrm{px}}+k_{g13y}{\stackrel{.}{W}}_{\mathrm{pz}}+{\mathrm{\ϵ}}_{\mathrm{gy}}---\left(5\right)$

Z gyroscopic drift error model is

${\mathrm{\ϵ}}_z=k_{g0z}+k_{g11z}{\stackrel{.}{W}}_{\mathrm{pz}}+k_{g12z}{\stackrel{.}{W}}_{\mathrm{px}}+k_{g13z}{\stackrel{.}{W}}_{\mathrm{py}}+{\mathrm{\ϵ}}_{\mathrm{gz}}---\left(6\right)$

In upper three formulas, ε _gx, ε _gy, ε _gzbe respectively X gyro, the random drift of Y gyro and Z gyro; k _g0ifor gyroscope drift, k _g11ifor the gyro input axis Monomial coefficient of correspondence, k _g12ifor the gyroscope axis of rotation Monomial coefficient of correspondence, k _g13ifor the gyro output axis Monomial coefficient of correspondence, (i represents x or y or z);

Therefore non-fully free degree Inertial Platform System state equation is obtained as follows

$\stackrel{.}{X}=f\left(X\right)+w=\left[\begin{array}{c}\mathrm{A\φ}+{\mathrm{Bk}}_g\\ 0_{24\×1}\end{array}\right]+w---\left(7\right)$

Wherein $X={\left[\begin{array}{ccc}{\mathrm{\φ}}^T& k_g^T& k_a^T\end{array}\right]}^T,\mathrm{\φ}={\left[\begin{array}{ccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z\end{array}\right]}^T,$

k _g＝[k _g0xk _g0yk _g0zk _g11xk _g11yk _g11zk _g12xk _g12yk _g12zk _g13xk _g13yk _g13z] ^T

k _a＝[k _a0xk _a0yk _a0zk _a1xk _a1yk _a1zθ _pxθ _pyθ _pzθ _oxθ _oyθ _oz] ^T

Wherein k _a0iinclined for adding table zero, k _a1ifor adding table scale factor, θ _pi, θ _oifor adding the alignment error shown around balance staff and output shaft; W is white Gaussian noise interference vector;

$A=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ez}}& -{\mathrm{\ω}}_{\mathrm{ey}}\\ -{\mathrm{\ω}}_{\mathrm{ez}}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ey}}& 0& 0\end{array}\right],$

$B=\left[\begin{array}{cccccccccccc}1& 0& 0& {\stackrel{.}{W}}_{\mathrm{px}}& 0& 0& {\stackrel{.}{W}}_{\mathrm{pz}}& 0& 0& {\stackrel{.}{W}}_{\mathrm{py}}& 0& 0\\ 0& 1& 0& 0& {\stackrel{.}{W}}_{\mathrm{py}}& 0& 0& {\stackrel{.}{W}}_{\mathrm{px}}& 0& 0& {\stackrel{.}{W}}_{\mathrm{pz}}& 0\\ 0& 0& 1& 0& 0& {\stackrel{.}{W}}_{\mathrm{pz}}& 0& 0& -{\stackrel{.}{W}}_{\mathrm{px}}& 0& 0& {\stackrel{.}{W}}_{\mathrm{py}}\end{array}\right]$

Accelerometer linear convergent rate model is:

Platform X accelerometer linear convergent rate model

$Z_{\mathrm{ax}}={\stackrel{.}{W}}_{\mathrm{px}}+k_{a0x}+k_{a1x}{\stackrel{.}{W}}_{\mathrm{px}}+{\mathrm{\θ}}_{\mathrm{px}}{\stackrel{.}{W}}_{\mathrm{py}}-{\mathrm{\θ}}_{\mathrm{ox}}{\stackrel{.}{W}}_{\mathrm{pz}}+e_{\mathrm{ax}}---\left(8\right)$

Platform Y accelerometer linear convergent rate model

$Z_{\mathrm{ay}}={\stackrel{.}{W}}_{\mathrm{py}}+k_{a0y}+k_{a1y}{\stackrel{.}{W}}_{\mathrm{py}}+{\mathrm{\θ}}_{\mathrm{py}}{\stackrel{.}{W}}_{\mathrm{pz}}-{\mathrm{\θ}}_{\mathrm{oy}}{\stackrel{.}{W}}_{\mathrm{px}}+e_{\mathrm{ay}}---\left(9\right)$

Platform Z accelerometer linear convergent rate model

$Z_{\mathrm{az}}={\stackrel{.}{W}}_{\mathrm{pz}}+k_{a0z}+k_{a1z}{\stackrel{.}{W}}_{\mathrm{pz}}+{\mathrm{\θ}}_{\mathrm{pz}}{\stackrel{.}{W}}_{\mathrm{px}}-{\mathrm{\θ}}_{\mathrm{oz}}{\stackrel{.}{W}}_{\mathrm{py}}+e_{\mathrm{az}}---\left(10\right)$

E _aifor adding table random error;

Then observational equation is

$Z=h\left(X\right)+v=\left[\begin{array}{c}{\mathrm{Ck}}_a+{\stackrel{.}{W}}_p\\ \mathrm{\φ}+{\mathrm{\φ}}_0\end{array}\right]+v---\left(11\right)$

Wherein

$Z=\left[\begin{array}{c}{Z}_a\\ Z_{\mathrm{an}}\end{array}\right],$

Z _afor three axis accelerometer exports, Z _anbe that three axle angular transducers export, φ ₀=[α ₀β ₀γ ₀] ^tfor the corner that diverse location is corresponding, v is 6 dimension white Gaussian noise interference vectors;

$C=\left[\begin{array}{cccccccccccc}1& 0& 0& {\stackrel{.}{W}}_{\mathrm{px}}& 0& 0& {\stackrel{.}{W}}_{\mathrm{py}}& 0& 0& -{\stackrel{.}{W}}_{\mathrm{pz}}& 0& 0\\ 0& 1& 0& 0& {\stackrel{.}{W}}_{\mathrm{py}}& 0& 0& {\stackrel{.}{W}}_{\mathrm{pz}}& 0& 0& {-\stackrel{.}{W}}_{\mathrm{px}}& 0\\ 0& 0& 1& 0& 0& {\stackrel{.}{W}}_{\mathrm{pz}}& 0& 0& {\stackrel{.}{W}}_{\mathrm{px}}& 0& 0& -{\stackrel{.}{W}}_{\mathrm{py}}\end{array}\right].$

Step 3, use the observability matrix of Lie derivatives method to the non-fully free degree Inertial Platform System that step 2 obtains to calculate according to selected location, judge that whether system is completely considerable;

The Lie Derivative Definition of nonlinear system is

$L_f^{k}h\left(X\right)=\frac{{\∂L}_f^{k-1}h\left(X\right)}{\∂X}f\left(X\right),L_f^{0}h\left(X\right)=h\left(X\right)---\left(12\right)$

Wherein k is differential times;

Defining vectorial L is

$L=\left[\begin{array}{c}h\left(X\right)\\ L_f^{1}h\left(X\right)\\ .\\ .\\ .\\ L_f^{n-1}h\left(X\right)\end{array}\right]=\left[\begin{array}{c}{L}_0\\ L_1\\ .\\ .\\ .\\ L_{n-1}\end{array}\right]---\left(13\right)$

Wherein n is the dimension of vectorial X, then the Jacobi matrix of L to X is

$M=\frac{\∂L}{\∂X}=\left[\begin{array}{ccccc}\frac{{\∂L}_0}{{\∂X}_1}& .& .& .& \frac{{\∂L}_0}{{\∂X}_n}\\ .& & & & .\\ .& & & & .\\ .& & & & .\\ \frac{{\∂L}_{n-1}}{{\∂X}_1}& .& .& .& \frac{{\∂L}_{n-1}}{{\∂X}_1}\end{array}\right]---\left(14\right)$

Detect for multiposition, the matrix that (14) formula of being brought into by each position design parameter obtains is designated as M _i, then the observability matrix of system is

P＝[M ₁M ₂… M _k].

If the order of P is less than n, again calculate observability matrix P after chosen position; If its order is n, then system is completely considerable, and then can detect whole error coefficient, namely carries out step 4;

Step 4, non-fully free degree Inertial Platform System is gone to the position chosen, export Jia Biao and angular transducer and carry out continuous acquisition, utilize step 2, the EKF filter system equation that step 3 obtains detects platform error angle and each error coefficient.