CN101545778A - Initial alignment method of electric control compass based on nonlinear filtering - Google Patents

Abstract

The invention provides an initial alignment method of an electric control compass based on nonlinear filtering, which relates to the technical problems of the initial alignment of the electric control compass, and the like. A UKF nonlinear error model for initial alignment of the electric control compass is established according to the characteristics of the gyroscope and the acceleration in the electric control compass. The method comprises the following steps: establishing a nonlinear initial alignment model of the electric control compass; and then carrying out the UKF filtering initial alignment of the electric control compass. An indirect output correcting filter structure is adopted by the UKF filtering of the electric control compass. The UKF optimal estimation state equation adopted in the electric control compass is x(k+1) = xk + g (xk) delta t + B omegak, and the observation noise equation is zk = h(xk) + vk. The initial alignment method can effectively solve the nonlinear problem of the large misalignment angle in initial alignment, thereby shortening the time of initial alignment and improving the performance of the whole system.

Description

A kind of electromagnet control gyrocompass initial alignment method based on nonlinear filtering

Technical field

The present invention relates to the technical matterss such as initial alignment in the equipment (following general designation " gyroscopic navigation equipment ") such as electromagnet control gyrocompass, platform compass, inertial navigation system, specifically be meant a kind of electromagnet control gyrocompass initial alignment method based on nonlinear filtering.

Background technology

Gyroscopic navigation equipment at first is in and prepares and the initial alignment state before startup enters operate as normal.Initial alignment is to set up the necessary starting condition of navigation equipment, as the initial attitude angle error of initial position, initial velocity, platform etc.The speed and the position of naval vessels obtain through integral and calculating, and this will provide starting condition V _X0, V _Y0, , λ ₀Because start at harbour, initial V _X0, V _Y0Be zero, , λ ₀Can have known measurement point to provide, perhaps provide with radio position-finder etc., but precision meet the demands should be within tens meters.Initial alignment can utilize the responsive error angle signal of the inertance element of gyroscopic navigation equipment own that system is calibrated, and designs a rational initial alignment scheme, and reduce error to satisfy request for utilization be very necessary as far as possible.The time of alignment precision and aligning is two important technology indexs when carrying out initial alignment.The initial alignment precision influences the performance of inertial navigation system, and the initial alignment time tag quick-reaction capability (QRC), therefore requires initial alignment precision height, aligning time weak point, and is promptly smart and fast.In order to reach this requirement, the initial alignment error model and the corresponding initial alignment Filtering Estimation method of realistic system be arranged.

Always there is nonlinear with different degree in real system, some system can be similar to and regard linear system as, but most systems then can not only be described with linear differential equation, nonlinear factor wherein can not be ignored, or for analysis integrated result better, must use the nonlinear mathematical model of reflection real system, the stochastic non-linear system filtering problem is arisen at the historic moment.

Have a large amount of random noises inevitably in the actual gyroscopic navigation equipment, traditional filtering method such as the high pass in the frequency domain, low pass, band are logical is powerless to this.The scholar had proposed to compose the Wiener filtering method of eliminating random noise by rated output afterwards.Though the Wiener filtering method is effective to random white noise, the calculated amount of this method is big, can't be suitable in real time complicated slightly system.Nineteen sixty, the Kalman has proposed the Kalman filtering algorithm based on state-space method and matrix operation, it provides strong tool for the optimal filtering of time domain recursion, it adopts time domain recursive algorithm, must storage time measurement amount in the process, thereby Kalman filtering is used widely in every field such as navigation, communication, remote sensing.But Kalman filtering can only be handled linear system.For solving the nonlinear problem in the engineering reality, the scholar had proposed improved Kalman filtering algorithm EKF (Extended KalmanFilter EKF) again afterwards, but existed not enough.Up to the present, though EKF is had numerous improving one's methods, block EKF, iteration EKF etc. as high-order.But the linearization error of EKF has reduced the accuracy of model, prolongation in time, precision is difficult to guarantee, sometimes even disperse, these defectives still are difficult to overcome, so the various countries navigation expert of circle is making great efforts to study new more effective nonlinear noise reduction processing method always.For this reason, utilize UKF method, open up new technological approaches for solving the navigation field nonlinear problem based on the statistics nonlinear transformation.

Summary of the invention

The objective of the invention is in order to overcome the deficiency of above-mentioned background, propose a kind of electromagnet control gyrocompass initial alignment method, improve initial alignment precision and the practical operation effect of rapid alignment time to reach based on nonlinear filtering.

At above-mentioned purpose, method proposed by the invention is set up the UKF nonlinearity erron model of electromagnet control gyrocompass initial alignment according to the characteristic of gyro in the electromagnet control gyrocompass and acceleration, obtain the optimal estimation of system's random noise effectively, guarantee the precision of compensation, shorten start-up time, its concrete grammar is:

1) foundation of the non-linear initial alignment model of electromagnet control gyrocompass

Electromagnet control gyrocompass adopts the fixing northern formula inertial navigation machinery layout that refers to.Complete error model will be considered the error model and the acceleration error model of gyro.Gyro error includes three calibration factor errors, the alignment errors on three axles on the axle; Contain three misalignment errors of three calibration factor sum of errors on the axle in the same accelerometer error.INS errors generally has: 3 platform misalignments (α, beta, gamma); 3 velocity error (δ V _x, δ V _y, δ V _z); Longitude, latitude, 3 site errors of height; Consider that gyro, all error model factors of accelerometer can reach 21 dimension state variables.21 dimension error states do not allow on computing velocity and complexity for electromagnet control gyrocompass.The inventor drops to 15 dimensions through dimension-reduction treatment by 21 dimensions, and filtering can reduce more than 40% computing time like this, and precision property is suitable.So, be necessary the error model of electromagnet control gyrocompass is simplified processing.Because of electromagnet control gyrocompass is a shipborne equipment, vertically to acceleration, speed, height can be ignored, and only need to calculate east orientation and north orientation acceleration error, and three-dimensional speed and position also are simplified to two dimensional surface.The initial alignment time is not long, and gyro error and North-East Bound acceleration all are reduced to white noise at random.According to above-mentioned, the error state of electromagnet control gyrocompass is defined as the vector of one 5 dimension: x=[α β γ δ V _xδ V _y] ^TAccording to physical construction layout in the electromagnet control gyrocompass, can obtain α, β, γ, the δ V of system alignment _x, δ V _yThe nonlinearity erron model:

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\α}}=-w_{\mathrm{ie}}\sin \mathrm{\γ}\cos \mathrm{\φ}+\mathrm{\β}{w}_{\mathrm{ie}}\sin \mathrm{\φ}-\mathrm{\δ}{V}_x/R_e+{\mathrm{\ϵ}}_x+w_x\\ \stackrel{\·}{\mathrm{\β}}=(1-\cos \mathrm{\γ})w_{\mathrm{ie}}\cos \mathrm{\φ}-\mathrm{\α}{w}_{\mathrm{ie}}\sin \mathrm{\φ}+\mathrm{\δ}{V}_x/R_e+{\mathrm{\ϵ}}_y+w_y\\ \stackrel{\·}{\mathrm{\γ}}=(-\mathrm{\βsi\; n\γ}+\mathrm{\α}\cos \mathrm{\γ})w_{\mathrm{ie}}\cos \mathrm{\φ}+\tan \mathrm{\φ\δ}{V}_x/R_e+{\mathrm{\ϵ}}_z+w_z\\ \mathrm{\δ}{\stackrel{\·}{V}}_x=-(\cos \mathrm{\γ}-1)a_x+a_y\sin \mathrm{\γ}-g(\mathrm{\β}\cos \mathrm{\γ}+\mathrm{\α}\sin \mathrm{\γ})\\ +2w_{\mathrm{ie}}\mathrm{\δ}{V}_y\sin \mathrm{\φ}+\▿y+w_{\mathrm{\δVx}}\\ \mathrm{\δ}{\stackrel{\·}{V}}_y=-\sin \mathrm{\γ}{a}_x+(\cos \mathrm{\γ}-1)a_y+g(-\mathrm{\β}\sin \mathrm{\γ}+\mathrm{\α}\cos \mathrm{\γ})\\ -2w_{\mathrm{ie}}\mathrm{\δ}{V}_x\sin \mathrm{\φ}+\▿x+w_{\mathrm{\δVy}}\end{array}\right.---\left(1\right)$

Following formula is rewritten as following form:

$\stackrel{\·}{X}=g\left(x\right)+\mathrm{B\ω}---\left(2\right)$

In the formula: x=[α β γ δ V _xδ V _y] ^T, w=[w _xw _yw _zw _{δ Vx}w _{δ Vy}] ^TIt is the random white noise of gyro and accelerometer; B=I _{5 * 5}R wherein _eIt is earth radius; ω _IeIt is earth rotational angular velocity; α, β, γ are that the platform error angle is three projections on the axle at navigation coordinate; δ V _x, δ V _yBe respectively latitude and North-East Bound velocity error; ε _x, ε _y, ε _zBe the projection of gyroscope constant value error in local geographic coordinate system oxyz,

Be that horizontal accelerometer often is worth biasing.

G () is non-linear, and it is carried out discretize, and establishing sampling interval is Δ t, and X (t+ Δ t) is located to carry out Taylor expansion at X (t), obtains:

$X(t+\mathrm{\Δt})=X\left(t\right)+g\left(x\right)\mathrm{\Δt}+\frac{\∂g\left(x\right)}{\∂x}g\left(x\right)\frac{{\left(\mathrm{\Δt}\right)}^2}{2}+...+\mathrm{B\ω}\left(t\right)---\left(3\right)$

Make X (t+ Δ t)=x _K+1, X (t)=x _k, ω (t)=ω _kAccording to accuracy requirement, ignore second order and the above a small amount of of second order of launching progression, the discrete equation that obtains system is:

x _k+1＝x _k+g(x _k)Δt+Bω _k (4)

Formula (4) is the non-linear initial alignment model of electromagnet control gyrocompass.

2) the UKF filtering initial alignment of electromagnet control gyrocompass

● state equation:

Indirect output calibration filter construction, its structural drawing such as Fig. 4 are adopted in the UKF filtering of electromagnet control gyrocompass.The UKF optimal estimation state equation that adopts in the electromagnet control gyrocompass is the depression of order error model of above-mentioned electromagnet control gyrocompass, sees formula (4).

● the error observation equation

Because the terrestrial magnetic field very easily is subjected on every side ferromagnetic magnetic interference over the ground, thereby Magnetic Sensor can produce than mistake, so precision is not high; And the geomagnetic observation of Magnetic Sensor is also to be a kind of autonomous observation, external information is relied on lack, and does not also need externally to carry out any communication interaction simultaneously, thereby also is used widely in the combination of navigational system in recent years.In the compass of being discussed, we have installed additional and the Magnetic Sensor measurement mechanism, the amount that can observe has course and cradle angle in length and breadth, the volume of Magnetic Sensor is little, as long as it is close with SINS as far as possible when installing, thereby the armed lever effect between Magnetic Sensor and SINS is very little, with respect to the bigger observational error of Magnetic Sensor, can ignore.

If the attitude of Magnetic Sensor output is respectively pitching R _Clj, rolling P _Clj, course H _Clj, the attitude angle of compass output is R _INS, P _INS, H _INSSo can be similar to and think and obtain being observed of attitude error angle

α＝R _clj-R _INS

＝P _clj-P _INS (5)

γ＝H _clj-H _INS

Formula (5) is out of shape, and makes Z=[R _CljP _CljH _Clj] ^T, have

$Z=\left[\begin{array}{c}{R}_{\mathrm{clj}}\\ P_{\mathrm{clj}}\\ H_{\mathrm{clj}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\\ \mathrm{\γ}\end{array}\right]+\left[\begin{array}{c}{R}_{\mathrm{INS}}\\ P_{\mathrm{INS}}\\ H_{\mathrm{INS}}\end{array}\right]+W_{\mathrm{clj}}---\left(6\right)$

W wherein _CljFor the error observation noise of Magnetic Sensor, can think the zero-mean white noise in the short time of aligning.To measure the equation brief note is

z _k＝h(x _k)+v _k (7)

V wherein _kBe observation noise.

● the UKF filtering algorithm of discrete recursion

At the error equation of the top electromagnet control gyrocompass of setting up and the system of observation noise equation (4) and (7) definition, be rewritten as following formula:

x(k+1)＝f[x(k)，u(k)，w(k)] (8)

z(k)＝h[x(k)，v(k)] (9)

In UKF filtering, owing to have noise item, need expand dimension to state and handle, make x ^a=[x ^Tw ^Tv ^T] ^T, concrete UKF filtering flow process is as follows:

The state starting condition is

${\hat{x}}_0=E\left(x_0\right)$

${\mathrm{<figure-callout\; id="0"\; label="P"\; filenames="A200810236875E00151.png,A200810236875E00153.png"\; state="\{\{state\}\}">P</figure-callout>}}_0=E\left\{(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right\}$

${\hat{x}}_0^a=E\left(x^a\right)={\left[\begin{array}{ccc}{x}_0^{\mathrm{<figure-callout\; id="0"\; label="T"\; filenames="A200810236875E00151.png,A200810236875E00153.png"\; state="\{\{state\}\}">T</figure-callout>}}& 0& 0\end{array}\right]}^T---\left(10\right)$

$P_0^a=E\left\{(x_0^a-{\hat{x}}_0^a){(x_0^a-{\hat{x}}_0^a)}^T\right\}=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="0"\; label="P"\; filenames="A200810236875E00151.png,A200810236875E00153.png"\; state="\{\{state\}\}">P</figure-callout>}}_0& 0& 0\\ 0& R^w& 0\\ 0& 0& R^v\end{array}\right]$

For k ∈ 1,2 .... ∞, calculate the sigma point rebuild:

${\mathrm{\&chi;}}_{k-1}^a=\left[\begin{array}{ccc}{\hat{x}}_{k-1}^a& {\hat{x}}_{k-1}^a+\mathrm{\&xi;}\sqrt{P_{k-1}^a}& {\hat{x}}_{k-1}^a-\mathrm{\&xi;}\sqrt{P_{k-1}^a}\end{array}\right]---\left(11\right)$

The predictive equation of UKF is:

${\mathrm{\&chi;}}_{k|k-1}^a=f({\mathrm{\&chi;}}_{k-1}^x,u_{k-1},{\mathrm{\&chi;}}_{k-1}^w)$

${\hat{x}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^m{\mathrm{\&chi;}}_{i,k|k-1}^x$

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^c[{\mathrm{\&chi;}}_{i,k|k-1}^x-{\hat{x}}_k^{-}]{[{\mathrm{\&chi;}}_{i,k|k-1}^x-{\hat{x}}_k^{-}]}^T---\left(12\right)$

$Z_{k|k-1}=h({\mathrm{\&chi;}}_{k|k-1}^a,{\mathrm{\&chi;}}_{k|k-1}^v)$

${\hat{z}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^m{Z}_{i,k|k-1}$

The renewal equation of UKF is:

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^c[Z_{i,k|k-1}-{\hat{z}}_k^{-}]{[z_{i,k|k-1}-{\hat{z}}_k^{-}]}^T$

$P_{x_k{z}_k}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^c[{\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-}]{[z_{i,k|k-1}-{\hat{z}}_k^{-}]}^T$

${\mathrm{\&kappa;}}_k=P_{x_k{z}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}$ (13)

${\hat{x}}_k={\hat{x}}_k^{-}+{\text{\&kappa;}}_k(z_k-{\hat{z}}_k^{-})$

$P_k=P_k^{-}-{\mathrm{\&kappa;}}_k{P}_{{\hat{z}}_k{\hat{z}}_k}{\mathrm{\&kappa;}}_k^T$

X wherein ^a=[x ^Tw ^Tv ^T] ^T, χ ^a=[(χ ^x) ^T(χ ^w) ^T(χ) ^T] ^T, the ξ list of values sigma point sampling method in the formula (11), the i.e. method of UT conversion.Weighting coefficient W wherein _iCalculating formula be

${W^{\mathrm{<figure-callout\; id="0"\; label="m"\; filenames="A200810236875E00151.png,A200810236875E00153.png"\; state="\{\{state\}\}">m</figure-callout>}}}_0=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;})$

$W_0^c=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;})+(1-{\mathrm{\&tau;}}^2+\mathrm{\&eta;})$ (14)

${W^m}_i={W^c}_i=1/\left\{2(L+\mathrm{\&lambda;})\right\}$

Wherein $\mathrm{\&xi;}=\mathrm{\&tau;}\sqrt{(L+k)},$ τ be a little positive number (be taken as 1 usually〉τ 〉=1e ^-4), determined the distribution of sigma point around x.λ=τ ²(L+k)-L is a scaling factor.K also is a scaling factor, often is taken as 0 or 3-L, and η is that η gets to the added parameter of the distribution of needle-like attitude vector x,

Be the i row of matrix square root.

Superiority of the present invention is:

Electromagnet control gyrocompass initial alignment test findings shows, utilize error model that system derives and observation equation to carry out UKF filtering initial alignment after, when reaching same accuracy requirement, the time of alignment of orientation is shortened in the 200s by original 2879s; Simultaneously the longitudinal and transverse misalignment that shakes also under same precision the initial alignment time go up original 2400s respectively and 2700s shortens to 220s and 180s; In addition, the speed of system is dispersed also with the accumulation of acceleration stochastic error and is improved greatly, and in 1500s, electromagnet control gyrocompass autoregistration speed diffuses to 14m/s as seen from Figure 5, and after adopting UKF filtering initial alignment, speed only diffuses to about 4m/s.

Analyze α, β, γ and speed (the North-East Bound velocity and) curve of its initial alignment testing experiment, can obtain to draw a conclusion:

(1) the initial alignment nonlinearity erron model of electromagnet control gyrocompass system derivation and UKF combination initial alignment algorithm can effectively solve non-linear that big misalignment exists in the initial alignment, thereby shorten the initial alignment time, and the total system performance is improved;

(2) electromagnet control gyrocompass adopts UKF initial alignment technology, can effectively suppress conventional static initial alignment down speed disperse phenomenon;

The initial alignment nonlinearity erron model that electromagnet control gyrocompass adopts, UKF combination initial alignment algorithm has important reference for other engineering practical applications such as optimal estimation of the navigational state error of big nonlinear problem that exists in other system and navigational system.

Description of drawings

Fig. 1 is the electromagnet control gyrocompass combined filter align structures block diagram based on UKF

Fig. 2 compares from the γ of initial alignment and UKF combination initial alignment in the embodiment

Fig. 3 compares from the β of initial alignment and UKF combination initial alignment in the embodiment

Fig. 4 compares from the α of initial alignment and UKF combination initial alignment in the embodiment

Fig. 5 be in the embodiment from the velocity ratio of initial alignment and UKF combination initial alignment

Embodiment

Describe performance of the present invention in detail below in conjunction with accompanying drawing, but they do not constitute the qualification to patent of the present invention, only do for example.Simultaneously by illustrating that advantage of the present invention will become clear more and understanding easily.

Embodiments of the invention:

According to the state equation and the observation equation of electromagnet control gyrocompass, according to discrete stepping type (9)-(12) of UKF, system carries out initial alignment again, and the scheme flow process of its realization is seen accompanying drawing 1.In system's initial alignment test, initial value is chosen: ${\hat{X}}_0={\left[\begin{array}{ccccc}\mathrm{\&alpha;}& \mathrm{\&beta;}& \mathrm{\&gamma;}& {\mathrm{\&delta;V}}_x& \mathrm{\&delta;}{V}_y\end{array}\right]}^T={\left[\begin{array}{ccccc}-1.2,& 1.2,& -25,& 0.2,& 0.2\end{array}\right]}^T,$ Angular unit is: °, speed unit is: m/s; Test is carried out in Chongqing factory, and its latitude is 30 °.By the error statistics characteristic of electromagnet control gyrocompass, calculate P according to the method for introducing in " Kalman filtering and integrated navigation principle " book ₀, getting each state-noise variance intensity more respectively is 5% of initial value.Given initial value

And P ₀, electromagnet control gyrocompass utilizes C/C++ in Control Software aforementioned error model and UKF to be realized, τ value 0.5, and η gets 2, and the filtering cycle is 0.1s, is respectively 20Hz and 10Hz to the sampling rate of electromagnet control gyrocompass and Magnetic Sensor.

In the static initial alignment test that the electromagnet control gyrocompass system carries out, the installation position benchmark of system is aimed at by optic theodolite optical and is guaranteed to aim at north orientation.System is carried out independent autoregistration and UKF combined filter aim at and compare, can obtain the result of accompanying drawing 2,3,4,5.In the experimental test, often be worth and be corrected after alignment error is measured by measuring method.In the test result of the autoregistration orientation misalignment γ of system and the UKF combined filter initial alignment of electromagnet control gyrocompass, orientation misalignment γ test result is seen Fig. 2.

Claims (4)

1. electromagnet control gyrocompass initial alignment method based on nonlinear filtering is characterized in that may further comprise the steps:

1) foundation of the non-linear initial alignment model of electromagnet control gyrocompass;

2) the UKF filtering initial alignment of electromagnet control gyrocompass.

2. a kind of electromagnet control gyrocompass initial alignment method based on nonlinear filtering according to claim 1 is characterized in that:

The method for building up of described electromagnet control gyrocompass nonlinearity erron model is as follows:

Electromagnet control gyrocompass adopts the fixing northern formula inertial navigation machinery layout that refers to, obtain comprising the error model of 21 dimension state variables of gyro error, accelerometer error, platform misalignment, velocity error, all factors of site error, carry out dimension-reduction treatment to 15 dimension then, get α, β, γ, δ V _x, δ V _yAs the error state variable, the nonlinearity erron model that obtains system alignment is as follows:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}=-w_{\mathrm{ie}}\sin \mathrm{\&gamma;}\cos \mathrm{\&phi;}+\mathrm{\&beta;}{w}_{\mathrm{ie}}\sin \mathrm{\&phi;}-\mathrm{\&delta;}{V}_x/R_e+{\mathrm{\&epsiv;}}_x+w_x\\ \stackrel{\&CenterDot;}{\mathrm{\&beta;}}=(1-\cos \mathrm{\&gamma;})w_{\mathrm{ie}}\cos \mathrm{\&phi;}-\mathrm{\&alpha;}{w}_{\mathrm{ie}}\sin \mathrm{\&phi;}+\mathrm{\&delta;}{V}_x/R_e+{\mathrm{\&epsiv;}}_y+w_y\\ \stackrel{\&CenterDot;}{\mathrm{\&gamma;}}=(-\mathrm{\&beta;}\sin \mathrm{\&gamma;}+\mathrm{\&alpha;}\cos \mathrm{\&gamma;})w_{\mathrm{ie}}\cos \mathrm{\&phi;}+\tan \mathrm{\&phi;\&delta;}{V}_x/R_e+{\mathrm{\&epsiv;}}_z+w_z\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_x=-(\cos \mathrm{\&gamma;}-1)a_x+a_y\sin \mathrm{\&gamma;}-g(\mathrm{\&beta;}\cos \mathrm{\&gamma;}+\mathrm{\&alpha;}\sin \mathrm{\&gamma;})\\ +2w_{\mathrm{ie}}\mathrm{\&delta;}{V}_y\sin \mathrm{\&phi;}+\&dtri;y+w_{\mathrm{\&delta;Vx}}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_y=-\sin \mathrm{\&gamma;}{a}_x+(\cos \mathrm{\&gamma;}-1)a_y+g(-\mathrm{\&beta;}\sin \mathrm{\&gamma;}+\mathrm{\&alpha;}\cos \mathrm{\&gamma;})\\ -2w_{\mathrm{ie}}\mathrm{\&delta;}{V}_x\sin \mathrm{\&phi;}+\&dtri;x+w_{\mathrm{\&delta;Vy}}\end{array}\right.---\left(1\right)$

Following formula is rewritten as following form:

$\stackrel{\&CenterDot;}{X}=g\left(X\right)+\mathrm{B\&omega;}---\left(2\right)$

In the formula: x=[α β γ δ V _xδ V _y] ^T, w=[w _xw _yw _zw _{δ Vx}w _{δ Vy}] ^TIt is the random white noise of gyro and accelerometer; B=I _{5 * 5}R wherein _eIt is earth radius; ω _IeIt is earth rotational angular velocity; α, β, γ are that the platform error angle is three projections on the axle at navigation coordinate; δ V _x, δ V _yBe respectively latitude and North-East Bound velocity error; ε _x, ε _y, ε _zBe the projection of gyroscope constant value error in local geographic coordinate system oxyz, Be that horizontal accelerometer often is worth biasing;

G () is non-linear, and it is carried out discretize, and establishing sampling interval is Δ t, and X (t+ Δ t) is located to carry out Taylor expansion at X (t), obtains

$X(t+\mathrm{\&Delta;t})=X\left(t\right)+g\left(x\right)\mathrm{\&Delta;t}+\frac{\&PartialD;g\left(x\right)}{\&PartialD;x}g\left(x\right)\frac{{\left(\mathrm{\&Delta;t}\right)}^2}{2}+...+\mathrm{Bw}\left(t\right)---\left(3\right)$

Make X (t+ Δ t)=x _K+1, X (t)=x _k, ω (t)=ω _kAccording to accuracy requirement, ignore second order and the above a small amount of of second order of launching progression, the discrete equation that obtains system is:

x _k+1＝x _k+g(x _k)Δt+Bω _k (4)

Formula (4) is the error model of electromagnet control gyrocompass.

3, a kind of electromagnet control gyrocompass initial alignment method based on nonlinear filtering according to claim 1 and 2 is characterized in that:

The described step of setting up UKF nonlinear filtering state equation and measurement equation is as follows:

1) sets up UKF filter state equation

The output calibration filter construction is adopted in electromagnet control gyrocompass UKF filtering, and state equation is the nonlinearity erron model equation (4) of electromagnet control gyrocompass;

2) set up UKF filtering measurement equation

If the attitude of Magnetic Sensor output is respectively pitching R _Clj, rolling R _Clj, course H _Clj, the attitude angle of compass output is R _INS, P _INS, H _INSSo can be similar to and think and obtain being observed of attitude error angle

α＝R _clj-R _INS

β＝P _clj-P _INS (5)

γ＝H _clj-H _INS

Formula (5) is out of shape, and makes z=[R _CljP _CljH _Clj] ^T, have

$z=\left[\begin{array}{c}{R}_{\mathrm{clj}}\\ P_{\mathrm{clj}}\\ H_{\mathrm{clj}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\&alpha;}\\ \mathrm{\&beta;}\\ \mathrm{\&gamma;}\end{array}\right]+\left[\begin{array}{c}{R}_{\mathrm{INS}}\\ P_{\mathrm{INS}}\\ H_{\mathrm{INS}}\end{array}\right]+W_{\mathrm{clj}}---\left(6\right)$

W wherein _CljFor the error observation noise of Magnetic Sensor, can think the zero-mean white noise in the short time of aligning; To measure the equation brief note is

z _k＝h(x _k)+v _k (7)

V wherein _kBe observation noise.

4. according to claim 1 or 2 or 3 described a kind of electromagnet control gyrocompass initial alignment methods, it is characterized in that based on nonlinear filtering:

The UKF filtering algorithm step of described discrete recursion is as follows:

Press the error model equation of electromagnet control gyrocompass and the system of observation noise equation (4) and (7) definition, be rewritten as following formula:

x(k+1)＝f[x(k)，u(k)，w(k)] (8)

z(k)＝h[x(k)，v(k)] (9)

In UKF filtering, owing to have noise item, need expand dimension to state and handle, make x ^a=[x ^Tw ^Tv ^T] ^T, concrete UKF filtering flow process is as follows:

The state starting condition is

${\hat{x}}_0=E\left(x_0\right)$

$P_0=E\left\{(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right\}$

${\hat{x}}_0^a=E\left(x^a\right)={\left[\begin{array}{ccc}{x}_0^T& 0& 0\end{array}\right]}^T---\left(10\right)$

$P_0^a=E\left\{(x_0^a-{\hat{x}}_0^a){(x_0^a-{\hat{x}}_0^a)}^T\right\}=\left[\begin{array}{ccc}{P}_0& 0& 0\\ 0& R^w& 0\\ 0& 0& R^v\end{array}\right]$

For k ∈ 1,2 .... ∞, calculate the sigma point rebuild:

${\mathrm{\&chi;}}_{k-1}^a=\left[\begin{array}{ccc}{\hat{x}}_{k-1}^a& {\hat{x}}_{k-1}^a+\mathrm{\&xi;}\sqrt{P_{k-1}^a}& {\hat{x}}_{k-1}^a-\mathrm{\&xi;}\sqrt{P_{k-1}^a}\end{array}\right]---\left(11\right)$

The predictive equation of UKF is:

${\mathrm{\&chi;}}_{k|k-1}^a=f({\mathrm{\&chi;}}_{k-1}^x,u_{k-1},{\mathrm{\&chi;}}_{k-1}^w)$

${\hat{x}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^m{\text{\&chi;}}_{i,k|k-1}^x$

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^c[{\mathrm{\&chi;}}_{i,k|k-1}^x-{\hat{x}}_k^{-}]{[{\mathrm{\&chi;}}_{i,k|k-1}^x-{\hat{x}}_k^{-}]}^T---\left(12\right)$

$Z_{k|k-1}=h({\mathrm{\&chi;}}_{k|k-1}^a,{\mathrm{\&chi;}}_{k|k-1}^v)$

${\hat{z}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^m{Z}_{i,k|k-1}$

The renewal equation of UKF is:

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^c[Z_{i,k|k-1}-{\hat{z}}_k^{-}]{[Z_{i,k|k-1}-{\hat{z}}_k^{-}]}^T$

$P_{x_k{z}_k}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^c[{\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-}]{[Z_{i,k|k-1}-{\hat{z}}_k^{-}]}^T$

$K_k=P_{x_k{z}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}$

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-{\hat{z}}_k^{-})$

$P_k=P_k^{-}-K_k{P}_{{\hat{z}}_k{\hat{z}}_k}{K}_k^T$ (13)

X wherein ^a=[x ^Tw ^Tv ^T] ^T, χ ^a=[(χ ^x) ^T(χ ^w) ^T(χ ^v) ^T] ^T, the ξ list of values sigma point sampling method in the formula (11), the i.e. method of UT conversion; Weighting coefficient W wherein _iCalculating formula be

${W^m}_0=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;})$

$W_0^c=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;})+(1-{\mathrm{\&tau;}}^2+\mathrm{\&eta;})$

${W^m}_i={W^c}_i=1/\left\{2(L+\mathrm{\&lambda;})\right\}$ (14)

Wherein $\mathrm{\&xi;}=\mathrm{\&tau;}\sqrt{(L+K)},$ τ be a little positive number (be taken as 1 usually〉τ 〉=1e ^-4), determined the distribution of sigma point around x.λ=τ ²(L+k)-L is a scaling factor.K also is a scaling factor, often is taken as 0 or 3-L, and η is that η gets 2 to the added parameter of the distribution of needle-like attitude vector x, Be the i row of matrix square root.

Images