CN104374405A - MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering - Google Patents

Abstract

The invention belongs to the field of navigation and in particular relates to an MEMS strapdown inertial navigation initial alignment method for statistical property uncertainty of measurement noise. The method comprises the following steps: determining initial longitude and latitude of a carrier by utilizing GPS; acquiring the acceleration data output by an MEMS accelerometer and magnetic field intensity data output by a magnetometer; selecting the initial filter value; acquiring angular velocity output by an MEMS gyroscope and acceleration data output by the MEMS accelerometer; and estimating the platform misalignment angle, thus finishing accurate initial alignment. According to the method disclosed by the invention, a measurement noise variance matrix is estimated and adjusted in real time by monitoring the information sequence change during iteration each time, the adaptive ability of the system is improved, and the filtering stability is improved on the basis that the filtering precision is guaranteed.

Description

A kind of MEMS inertial navigation Initial Alignment Method based on self-adaptation central difference Kalman filtering

Technical field

The invention belongs to navigation field, be specifically related to a kind of to measurement noise statistical property uncertain MEMS inertial navigation Initial Alignment Method.

Background technology

MEMS (Micro Electronic Mechanical System-MEMS), again referred to as MEMS (micro electro mechanical system), is the microelectromechanicdevice device of new generation adopting nanometer technology to process.It is processing object with silicon semiconductor material, and the physical dimension taking special IC fabrication technique to go out has driving millimeter magnitude, controls and the microdevice of signal processing function.MEMS Inertial Measurement Unit is a kind of inertial measuring unit comprising 3 axis MEMS gyro and 3 axis MEMS accelerometer, and wherein gyro is mutually vertical with three axles of accelerometer, meets the right-hand rule.In view of MEMS has, volume is little, lightweight, power consumption is few, cost is low, integration degree advantages of higher, and it will have more wide future in engineering applications, especially for microminiature carrier navigation, guidance and gesture stability significant.

Initial alignment is one of gordian technique in strapdown inertial navitation system (SINS), and its aligning time and precision directly affect the serviceability of inertial navigation system, and the fundamental purpose of strapdown inertial navitation system (SINS) initial alignment sets up the initial value of attitude matrix.Initial alignment process mainly comprises coarse alignment and two stages of fine alignment, first by the size utilizing analytical method to roughly estimate misalignment, secondly, set up error state equation, because MEMS inertia device precision is not high, at the end of coarse alignment, course error angle is larger, and therefore Initial Alignment Error state equation is nonlinear, finally, filtering optimal estimation method is utilized accurately to estimate the size of misalignment thus realize fine alignment.

The basis of CDKF (Central difference Kalman filter) wave filter is central difference conversion, utilize the non-linear transformation method of interpolation theory, solve the statistic such as average and variance of stochastic variable, CDKF with Sterling interpolation formula for starting point, avoid complicated derivative operation by the method for approximation by polynomi-als nonlinear equation derivative, adopt the single order in central difference replacement Taylor series expansion and second derivative.CDKF is higher than traditional nonlinear filtering EKF precision, good stability, and the Jacobian matrix not needing calculation of complex; Compared with the same nonlinear filtering UKF based on Sigma point, have slightly high theoretical precision, but when practical application, CDKF can be subject to the uncertain and external environment of system noise to the impact of Filtering Model disturbing factor.System noise main source accelerometer bias and gyroscopic drift, i.e. inertial measurement component error.Due to before the general initial alignment of inertial device error all through Experimental modeling and compensation, therefore in actual alignment procedures, can ignore the impact of the state model of system; Consider on time, the difference of working environment, measurement noise is also different, and the severe degree of working environment will directly affect the performance of measurement noise, and uncertainty is very large, therefore needs to estimate in real time measurement noise.Adaptive estimation method is incorporated in CDKF algorithm, thus ensures that CDKF still can keep normal convergence when measurement noise statistical model is inaccurate, and there is higher estimated accuracy, improve system stability.

Summary of the invention

The object of the invention is to solve when measurement noise statistical model is inaccurate, a kind of MEMS inertial navigation Initial Alignment Method based on self-adaptation central difference Kalman filtering is provided.

The object of the present invention is achieved like this:

(1) GPS is utilized to determine initial longitude, the latitude of carrier;

(2) gather the acceleration information of mems accelerometer output and the magnetic field strength date of magnetometer output, utilize analytical method to carry out coarse alignment, obtain initial matrix determine attitude of carrier information and pitch angle, roll angle, course angle, wherein b represents carrier coordinate system, and n ' representative calculates navigational coordinate system;

(3) filtering initial value is chosen

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

Wherein x ₀for the initial value of state variable, P ₀for the initial error covariance matrix of state variable;

(4) angular velocity data of MEMS gyro instrument output and the acceleration information of mems accelerometer output is gathered;

(5) utilize self-adaptation central difference kalman filter method to carry out filtering, estimate the misaligned angle of the platform;

(6) initial matrix of the misaligned angle of the platform correction MEMS strapdown inertial navitation system (SINS) utilizing step (5) to estimate obtain accurate initial matrix namely complete accurate initial alignment, in other, n represents navigational coordinate system.

Initial matrix

θ ₀pitch angle, γ ₀roll angle, it is course angle.

State variable comprises the misalignment in coordinate system three directions and the velocity error of coordinate system horizontal direction.

Step (5) comprising:

(5.1) weights are determined:

$w_i=\left\{\begin{array}{cc}(h^2-m)/h^2& (i=0)\\ 1/\left(2h^2\right)& (i=\mathrm{1,2},...,2m)\end{array}\right.$

Wherein difference step size centered by h, be Gaussian distribution, optimal estimation value is m is the dimension of system state variables;

(5.2) the Sigma point of 2m+1 dimension is constructed:

${\mathrm{\ξ}}_{k-1}=[{\hat{x}}_{k-1},{\hat{x}}_{k-1}+h\sqrt{P_{k-1}},{\hat{x}}_{k-1}+h\sqrt{P_{k-1}}],$

Wherein for the state variable that the k-1 moment estimates, P _k-1for the error co-variance matrix of k-1 moment state variable;

(5.3) under state equation is non-linear, that measurement equation is linear condition, time renewal is carried out:

γ _k/k-1＝f _k-1(ξ _k-1)；

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i{\mathrm{\γ}}_{i,k/k-1};$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i({\mathrm{\γ}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\γ}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1},$

By and P _k-1calculate Sigma point ξ _k-1, by nonlinear state function f _k-1() propagates as γ _k/k-1, by γ _k/k-1status predication can be obtained with status predication error covariance matrix P _k/k-1, Q _k-1for system noise;

(5.4) under state equation is non-linear, that measurement equation is linear condition, carry out measurement upgrade:

$e_k=Z_k-H{\hat{x}}_{k/k-1}$

$d_{k-1}=\frac{1-l}{1-l^k}(0<l<1)$

${\hat{R}}_k=(1-d_{k-1}){\hat{R}}_{k-1}+d_{k-1}(e_k{e}_k^T-H{P}_{k/k-1}{H}^T)$

K _k＝P _k/k-1H ^T(HP _k/k-1H ^T+R _k) ^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H_k{\hat{x}}_{k/k-1})$

P _k＝(I-K _kH)P _k/k-1

Wherein, K _kfor filter gain matrix, e _kfor new breath, for state estimation is comprising the misaligned angle of the platform, P _kfor state error covariance matrix, for the On-line Estimation of measurement noise, l is forgetting factor, and span is 0.95 ~ 0.99.

State equation is: $\stackrel{.}{X}=f\left(X\right)+\mathrm{GW},$ System noise vector $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

Factor arrays $G=\left[\begin{array}{cc}{G}_1& 0_{3\&times;2}\\ 0_{2\&times;3}& G_2\end{array}\right],G_1=\left[\begin{array}{ccc}{C}_{11}^{\text{'}}& C_{12}^{\text{'}}& C_{13}^{\text{'}}\\ C_{21}^{\text{'}}& C_{22}^{\text{'}}& C_{23}^{\text{'}}\\ C_{31}^{\text{'}}& C_{32}^{\text{'}}& C_{33}^{\text{'}}\end{array}\right]G_2=\left[\begin{array}{cc}{C}_{11}^{\text{'}}& C_{21}^{\text{'}}\\ C_{21}^{\text{'}}& C_{22}^{\text{'}}\end{array}\right],$

F (X) is

${\stackrel{.}{\mathrm{\&phi;}}}_x=-\left(\sin {\mathrm{\&phi;}}_z\right){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&phi;}}_y{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\&delta;}{v}_y}{R_M}+C_{11}^{\text{'}}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\text{'}}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\text{'}}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\&phi;}}}_y=(1-\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-{\mathrm{\&phi;}}_x{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\&delta;}{v}_y}{R_N}+C_{21}^{\text{'}}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\text{'}}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\text{'}}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\&phi;}}}_z=(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\&delta;}{v}_x\tan L}{R_N}+C_{31}^{\text{'}}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\text{'}}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\text{'}}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$ $\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\&phi;}}_z-1)+f_y\sin {\mathrm{\&phi;}}_z-f_z({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_y\sin L+\\ C_{11}^{\text{'}}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\text{'}}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_y=-f_x\sin {\mathrm{\&phi;}}_z-f_y(\cos {\mathrm{\&phi;}}_z-1){-f}_z({\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z-{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_x\sin L+\\ C_{21}^{\text{'}}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\text{'}}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

Wherein, R _m, R _nbe respectively earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches, φ _x, φ _y, φ _zit is the misaligned angle of the platform in three directions; δ v _x, δ v _yfor velocity error; L is local latitude; w _iefor rotational-angular velocity of the earth; be the gyroscopic drift of three axis; for gyro zero mean Gaussian white noise; the acceleration zero being three axis is inclined; for accelerometer zero mean Gaussian white noise; f _x, f _y, f _zfor acceleration exports the value of specific force on computed geographical coordinates; C ' _ijthat carrier is tied to the element calculated in Department of Geography's matrix;

Measurement equation is:

Z＝HX+V

Wherein, measurement amount Z is inertial navigation horizontal velocity error delta v _x, δ v _y; H=[I _{2 × 2}0 _{2 × 3}], I _{2 × 2}for unit second-order matrix, 0 _{2 × 3}be 2 row 3 row null matrix; V is measurement noise.

Beneficial effect of the present invention is: in the system state space model process that the present invention sets up, misalignment and velocity error model all adopt non-linear form, can describe actual in MEMS strapdown inertial navitation system (SINS) error Propagation Property so accurately; Traditional CDKF is a kind of very outstanding non-linear filtering method, but, the state equation of SINS large misalignment angle initial alignment has non-linear, and measurement equation has linear feature, therefore can improve CDKF, namely Sigma conversion is adopted to state equation, linear renewal is taked to measurement equation, owing to no matter carrying out Sigma conversion or linear transformation for linear model, precision after conversion is identical, so linearly upgrade measurement equation, while keeping filtering accuracy, shorten the time of initial alignment; First auto adapted filtering is joined in CDKF filtering method in initial alignment process, self-adaptation CDKF filtering method is in the inaccurate situation of measurement noise statistical property, when each iteration by the change of monitoring innovation sequence, real-time Estimation and rectification measuring noise square difference battle array, add the adaptive ability of system, the basis ensureing filtering accuracy improves filter wave stability.

Accompanying drawing explanation

Fig. 1 represents the estimated accuracy schematic diagram of roll angle.

Fig. 2 represents the estimated accuracy schematic diagram of pitch angle.

Fig. 3 represents azimuthal estimated accuracy schematic diagram.

Embodiment

Below in conjunction with accompanying drawing, the present invention is described further.

Invention is achieved through the following technical solutions above-mentioned purpose, comprises the following steps: 1, utilize GPS to determine initial longitude, the latitude parameter of carrier;

2, gather the data of mems accelerometer and magnetometer output, utilize analytical method to carry out coarse alignment, tentatively determine attitude of carrier information

θ in formula ₀, γ ₀, pitch angle respectively, roll angle, course angle;

3, according to the error characteristics of inertial navigation, Large azimuth angle Nonlinear Filtering Formulae is set up, i.e. state equation and measurement equation;

4, filtering initial value is chosen

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

Wherein x ₀for the initial value of state variable, P ₀for the initial error covariance matrix of state variable;

5, the data of MEMS gyro instrument and accelerometer output are gathered;

6, utilize self-adaptation CDKF method to carry out filtering and estimate misalignment, specific as follows:

(1) weights are determined

$w_i=\left\{\begin{array}{cc}(h^2-n)/h^2& (i=0)\\ 1/\left(2h^2\right)& (i=\mathrm{1,2},...,2n)\end{array}\right.$

Wherein difference step size centered by h, for the optimal estimation value of Gaussian distribution h n is the dimension of system state variables;

(2) the Sigma point of 2n+1 dimension is constructed

${\mathrm{\&xi;}}_{k-1}=[{\hat{x}}_{k-1},{\hat{x}}_{k-1}+h\sqrt{P_{k-1}},{\hat{x}}_{k-1}+h\sqrt{P_{k-1}}]$

Wherein for the state estimation in k-1 moment, P _k-1for the state error covariance matrix in k-1 moment;

(3) under state equation is non-linear, that measurement equation is linear condition, time renewal is carried out

γ _k/k-1＝f _k-1(ξ _k-1)；

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i{\mathrm{\&gamma;}}_{i,k/k-1};$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1}$

According to the Sigma sampling policy selected by (2) step, by and P _k-1calculate Sigma point ξ _k-1, by nonlinear state function f _k-1() propagates as γ _k/k-1, by γ _k/k-1status predication can be obtained with predicting covariance battle array P _k/k-1, Q _k-1for system noise;

(4) under state equation is non-linear, that measurement equation is linear condition, carry out measurement upgrade

$e_k=Z_k-H{\hat{x}}_{k/k-1}$

$d_{k-1}=\frac{1-b}{1-b^k}(0<b<1)$

${\hat{R}}_k=(1-d_{k-1}){\hat{R}}_{k-1}+d_{k-1}(e_k{e}_k^T-H{P}_{k/k-1}{H}^T)$

K _k＝P _k/k-1H ^T(HP _k/k-1H ^T+R _k) ^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H_k{\hat{x}}_{k/k-1})$

P _k＝(I-K _kH)P _k/k-1

Wherein, K _kfor filter gain matrix, e _kfor new breath, for state estimation, P _kfor state error covariance matrix, for the On-line Estimation of measurement noise.B is forgetting factor, and general span is 0.95 ~ 0.99.Adopt forgetting factor can the memory span of restriction filter, strengthen observation data recently and, to the current effect estimated, most recent data is played a role in the estimation, and stale data is forgotten gradually.For linear equation, no matter be Sigma conversion or linear transformation, the precision after conversion is identical, but, from calculated amount, measure average in calculating time, new algorithm is not but again adopted 2n Sigma point and is carried out nonlinear transformation, carries out linear transformation like this can reduce calculated amount when measurement equation is linear equation;

The strapdown initial matrix of the misaligned angle of the platform update the system 7, utilizing step 6 to estimate obtain accurate initial strap-down matrix namely thus complete accurate initial alignment;

Embodiment 1

1, GPS is utilized to determine initial longitude, the latitude parameter of carrier;

2, the data of mems accelerometer and magnetometer output are gathered; Utilize analytical method to carry out coarse alignment, rough obtains the attitude square of carrier coordinate system to computed geographical coordinates first the output signal of mems accelerometer is utilized to determine pitch angle and the roll angle of carrier, its principle is under quiet pedestal condition, the horizontal direction of acceleration of gravity in geographic coordinate system do not have projection components, only have to measure on Z axis and export, and then utilize magnetometer to output signal the course angle determining carrier.

θ in formula ₀, γ ₀, pitch angle respectively, roll angle, course angle;

3, coarse alignment terminates rear usual horizontal misalignment is little misalignment, and azimuthal misalignment angle is large misalignment angle, therefore sets up strapdown inertial navigation system initial alignment nonlinear state equation system noise vector $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

Factor arrays $G=\left[\begin{array}{cc}{G}_1& 0_{3\&times;2}\\ 0_{2\&times;3}& G_2\end{array}\right],G_1=\left[\begin{array}{ccc}{C}_{11}^{\text{'}}& C_{12}^{\text{'}}& C_{13}^{\text{'}}\\ C_{21}^{\text{'}}& C_{22}^{\text{'}}& C_{23}^{\text{'}}\\ C_{31}^{\text{'}}& C_{32}^{\text{'}}& C_{33}^{\text{'}}\end{array}\right]G_2=\left[\begin{array}{cc}{C}_{11}^{\text{'}}& C_{21}^{\text{'}}\\ C_{21}^{\text{'}}& C_{22}^{\text{'}}\end{array}\right],$ F (X) is

${\stackrel{.}{\mathrm{\&phi;}}}_x=-\left(\sin {\mathrm{\&phi;}}_z\right){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&phi;}}_y{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\&delta;}{v}_y}{R_M}+C_{11}^{\text{'}}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\text{'}}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\text{'}}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\&phi;}}}_y=(1-\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-{\mathrm{\&phi;}}_x{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\&delta;}{v}_y}{R_N}+C_{21}^{\text{'}}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\text{'}}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\text{'}}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\&phi;}}}_z=(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\&delta;}{v}_x\tan L}{R_N}+C_{31}^{\text{'}}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\text{'}}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\text{'}}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\&phi;}}_z-1)+f_y\sin {\mathrm{\&phi;}}_z-f_z({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_y\sin L+\\ C_{11}^{\text{'}}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\text{'}}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_y=-f_x\sin {\mathrm{\&phi;}}_z-f_y(\cos {\mathrm{\&phi;}}_z-1){-f}_z({\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z-{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_x\sin L+\\ C_{21}^{\text{'}}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\text{'}}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

Wherein, R _m, R _nbe respectively earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches, φ _x, φ _y, φ _zit is the misalignment (quantity of state) in three directions; δ v _x, δ v _yfor velocity error (quantity of state); L is local latitude; w _iefor rotational-angular velocity of the earth; be the gyroscopic drift of three axis; for gyro zero mean Gaussian white noise; the acceleration zero being three axis is inclined; for accelerometer zero mean Gaussian white noise; f _x, f _y, f _zfor acceleration exports the value of specific force on computed geographical coordinates; that carrier is tied to the element calculated in Department of Geography's matrix;

Initial alignment measurement equation is:

Z＝HX+V

Wherein, measurement amount Z is inertial navigation horizontal velocity error delta v _x, δ v _y; H=[I _{2 × 2}0 _{2 × 3}] (I _{2 × 2}for unit second-order matrix, 0 _{2 × 3}be 2 row 3 row null matrix); V is measurement noise;

4, filtering initial value is chosen

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

Wherein x ₀for the initial value of state variable, P ₀for the initial error covariance matrix of state variable;

5, the data of MEMS gyro instrument and accelerometer output are gathered;

6, utilize self-adaptation CDKF method to carry out filtering and estimate misalignment, specific as follows:

(1) weights are determined

$w_i=\left\{\begin{array}{cc}(h^2-n)/h^2& (i=0)\\ 1/\left(2h^2\right)& (i=\mathrm{1,2},...,2n)\end{array}\right.$

Wherein difference step size centered by h, for the optimal estimation value of Gaussian distribution h n is the dimension of system state variables;

(2) the Sigma point of 2n+1 dimension is constructed

${\mathrm{\&xi;}}_{k-1}=[{\hat{x}}_{k-1},{\hat{x}}_{k-1}+h\sqrt{P_{k-1}},{\hat{x}}_{k-1}+h\sqrt{P_{k-1}}]$

Wherein for the state estimation in k-1 moment, P _k-1for the evaluated error covariance matrix in k-1 moment;

(3) under state equation is non-linear, that measurement equation is linear condition, time renewal is carried out

Sigma point propagates γ _k/k-1=f _k-1(ξ _k-1)

Status predication ${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i{\mathrm{\&gamma;}}_{i,k/k-1}$

Predicated error $P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1}$

According to the Sigma sampling policy selected by (2) step, by and P _k-1calculate Sigma point ξ _k-1, by nonlinear state function f _k-1() propagates as γ _k/k-1, by γ _k/k-1status predication can be obtained with predicting covariance battle array P _k/k-1, Q _k-1for system noise;

(4) under state equation is non-linear, that measurement equation is linear condition, carry out measurement upgrade

Calculate new breath $e_k=Z_k-H{\hat{x}}_{k/k-1}$

Calculate weighting coefficient $d_{k-1}=\frac{1-b}{1-b^k}(0<b<1)$

The On-line Estimation of measurement noise ${\hat{R}}_k=(1-d_{k-1}){\hat{R}}_{k-1}+d_{k-1}(e_k{e}_k^T-H{P}_{k/k-1}{H}^T)$

Calculation of filtered gain matrix K _k=P _k/k-1h ^t(HP _k/k-1h ^t+ R _k) ^-1

State estimation ${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H_k{\hat{x}}_{k/k-1})$

Evaluated error P _k=(I-K _kh) P _k/k-1

Wherein, b is forgetting factor, and general span is 0.95 ~ 0.99.Adopt forgetting factor can the memory span of restriction filter, strengthen observation data recently and, to the current effect estimated, most recent data is played a role in the estimation, and stale data is forgotten gradually;

7, the strapdown initial matrix of the misaligned angle of the platform update the system estimated is utilized obtain accurate initial strap-down matrix namely thus complete accurate initial alignment;

8, matlab simulating, verifying is carried out to the method in the present invention:

Suppose carrier initial position: north latitude 45.7996 °, east longitude 126.6705 °, MEMS gyro instrument constant value drift 5 °/h, mems accelerometer zero inclined 10 ^-4g ₀, gravity acceleration g ₀=9.78049 (m/s ²), initial misalignment φ _x=1 °, φ _y=1 °, φ _z=10 °, earth radius R=6378393m.According to set parameter, utilize self-adaptation CDKF non-linear filtering method of the present invention can obtain Fig. 1, the roll angle shown in Fig. 2 Fig. 3, pitch angle, course angle.Result shows at Large azimuth angle and can obtain very high alignment precision under the inaccurate condition of measurement noise, improves filter wave stability.

Claims (5)

1., based on a MEMS inertial navigation Initial Alignment Method for self-adaptation central difference Kalman filtering, it is characterized in that:

(1) GPS is utilized to determine initial longitude, the latitude of carrier;

(2) gather the acceleration information of mems accelerometer output and the magnetic field strength date of magnetometer output, utilize analytical method to carry out coarse alignment, obtain initial matrix determine attitude of carrier information and pitch angle, roll angle, course angle, wherein b represents carrier coordinate system, and n ' representative calculates navigational coordinate system;

(3) filtering initial value is chosen

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

Wherein x ₀for the initial value of state variable, P ₀for the initial error covariance matrix of state variable;

(4) angular velocity data of MEMS gyro instrument output and the acceleration information of mems accelerometer output is gathered;

(5) utilize self-adaptation central difference kalman filter method to carry out filtering, estimate the misaligned angle of the platform;

(6) initial matrix of the misaligned angle of the platform correction MEMS strapdown inertial navitation system (SINS) utilizing step (5) to estimate obtain accurate initial matrix namely complete accurate initial alignment, in other, n represents navigational coordinate system.

2. a kind of MEMS inertial navigation Initial Alignment Method based on self-adaptation central difference Kalman filtering according to claim 1, is characterized in that: described initial matrix

θ ₀pitch angle, γ ₀roll angle, it is course angle.

3. a kind of MEMS inertial navigation Initial Alignment Method based on self-adaptation central difference Kalman filtering according to claim 2, is characterized in that: described state variable comprises the misalignment in coordinate system three directions and the velocity error of coordinate system horizontal direction.

4. a kind of MEMS inertial navigation Initial Alignment Method based on self-adaptation central difference Kalman filtering according to claim 3, it is characterized in that, described step (5) comprising:

(5.1) weights are determined:

$w_i=\left\{\begin{array}{cc}(h^2-m)/h^2& (i=0)\\ 1/\left({2h}^2\right)& (i=\mathrm{1,2},...,2m)\end{array}\right.$

Wherein difference step size centered by h, be Gaussian distribution, optimal estimation value is m is the dimension of system state variables;

(5.2) the Sigma point of 2m+1 dimension is constructed:

${\mathrm{\&xi;}}_{k-1}=[{\hat{x}}_{k-1},{\hat{x}}_{k-1}+h\sqrt{P_{k-1}},{\hat{x}}_{k-1}+h\sqrt{P_{k-1}}],$

Wherein for the state variable that the k-1 moment estimates, P _k-1for the error co-variance matrix of k-1 moment state variable;

(5.3) under state equation is non-linear, that measurement equation is linear condition, time renewal is carried out:

γ _k/k-1＝f _k-1(ξ _k-1)；

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i{\mathrm{\&gamma;}}_{i,k/k-1};$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_i({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\&gamma;}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1},$

By and P _k-1calculate Sigma point ξ _k-1, by nonlinear state function f _k-1() propagates as γ _k/k-1, by γ _k/k-1status predication can be obtained with status predication error covariance matrix P _k/k-1, Q _k-1for system noise;

(5.4) under state equation is non-linear, that measurement equation is linear condition, carry out measurement upgrade:

$e_k=Z_k-H{\hat{x}}_{k/k-1}$

$d_{k-1}=\frac{1-l}{1-l^k}(0<l<1)$

${\hat{R}}_k=(1-d_{k-1}){\hat{R}}_{k-1}+d_{k-1}(e_k{e}_k^T-{\mathrm{HP}}_{k/k-1}{H}^T)$

K _k＝P _k/k-1H ^T(HP _k/k-1H ^T+R _k) ^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H_k{\hat{x}}_{k/k-1})$

P _k＝(I-K _kH)P _k/k-1

Wherein, K _kfor filter gain matrix, e _kfor new breath, for state estimation is comprising the misaligned angle of the platform, P _kfor state error covariance matrix, for the On-line Estimation of measurement noise, l is forgetting factor, and span is 0.95 ~ 0.99.

5. a kind of MEMS inertial navigation Initial Alignment Method based on self-adaptation central difference Kalman filtering according to claim 4, it is characterized in that, described state equation is:

$\stackrel{.}{X}=f\left(X\right)+\mathrm{GW},$ System noise vector $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

Factor arrays $G=\left[\begin{array}{cc}{G}_1& 0_{3\&times;2}\\ 0_{2\&times;3}& G_2\end{array}\right]\text{,}{G}_1=\left[\begin{array}{ccc}{C}_{11}^{\&prime;}& C_{12}^{\&prime;}& C_{13}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}& C_{23}^{\&prime;}\\ C_{31}^{\&prime;}& C_{32}^{\&prime;}& C_{33}^{\&prime;}\end{array}\right]G_2=\left[\begin{array}{cc}{C}_{11}^{\&prime;}& C_{12}^{\&prime;}\\ C_{21}^{\&prime;}& C_{22}^{\&prime;}\end{array}\right],$

F (X) is

${\stackrel{.}{\mathrm{\&phi;}}}_x=-\left(\sin {\mathrm{\&phi;}}_z\right){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&phi;}}_y{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\frac{{\mathrm{\&delta;v}}_y}{R_M}+C_{11}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\&phi;}}}_y=(1-\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-{\mathrm{\&phi;}}_x{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\&delta;}{v}_x}{R_N}+C_{21}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\&phi;}}}_z=(-{\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{{\mathrm{\&delta;v}}_x\tan L}{R_N}+C_{31}^{\&prime;}({\mathrm{\&epsiv;}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\&prime;}({\mathrm{\&epsiv;}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\&prime;}({\mathrm{\&epsiv;}}_z^b+w_{\mathrm{gz}}^b)$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\&phi;}}_z-1)+f_y\sin {\mathrm{\&phi;}}_z-f_z({\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_z+{\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_y\sin L+\\ C_{11}^{\&prime;}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\&prime;}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{.}{v}}_y=-f_x\sin {\mathrm{\&phi;}}_z-f_y(\cos {\mathrm{\&phi;}}_z-1)-f_z({\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_z-{\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\mathrm{\&delta;}{v}_x\sin L+\\ C_{21}^{\&prime;}({\&dtri;}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\&prime;}({\&dtri;}_y^b+w_{\mathrm{ay}}^b)\end{array}$

Wherein, R _m, R _nbe respectively earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches, φ _x, φ _y, φ _zit is the misaligned angle of the platform in three directions; δ v _x, δ v _yfor velocity error; L is local latitude; w _iefor rotational-angular velocity of the earth; be the gyroscopic drift of three axis; for gyro zero mean Gaussian white noise; the acceleration zero being three axis is inclined; for accelerometer zero mean Gaussian white noise; f _x, f _y, f _zfor acceleration exports the value of specific force on computed geographical coordinates; C ' _ijthat carrier is tied to the element calculated in Department of Geography's matrix;

Measurement equation is:

Z＝HX+V

Wherein, measurement amount Z is inertial navigation horizontal velocity error delta v _x, δ v _y; H=[I _{2 × 2}0 _{2 × 3}], I _{2 × 2}for unit second-order matrix, 0 _{2 × 3}be 2 row 3 row null matrix; V is measurement noise.