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

MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering Download PDF

Info

Publication number
CN104374405A
CN104374405A CN201410624222.0A CN201410624222A CN104374405A CN 104374405 A CN104374405 A CN 104374405A CN 201410624222 A CN201410624222 A CN 201410624222A CN 104374405 A CN104374405 A CN 104374405A
Authority
CN
China
Prior art keywords
phi
prime
sin
matrix
mems
Prior art date
Application number
CN201410624222.0A
Other languages
Chinese (zh)
Inventor
王通达
宋春雨
李美玲
徐英蛟
刘萍
于天琦
Original Assignee
哈尔滨工程大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 哈尔滨工程大学 filed Critical 哈尔滨工程大学
Priority to CN201410624222.0A priority Critical patent/CN104374405A/en
Publication of CN104374405A publication Critical patent/CN104374405A/en

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

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

x ^ 0 = E [ x 0 ] , P 0 = E [ ( x 0 - x ^ 0 ) ( x 0 - x ^ 0 ) T ]

Wherein x 0for the initial value of state variable, P 0for 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

θ 0pitch angle, γ 0roll 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 = ( h 2 - m ) / h 2 ( i = 0 ) 1 / ( 2 h 2 ) ( i = 1,2 , . . . , 2 m )

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:

ξ k - 1 = [ x ^ k - 1 , x ^ k - 1 + h P k - 1 , x ^ k - 1 + h 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-1k-1);

x ^ k / k - 1 = Σ i = 0 2 n w i γ i , k / k - 1 ;

P k / k - 1 = Σ i = 0 2 n w i ( γ i , k / k - 1 - x ^ k / k - 1 ) ( γ i , k / k - 1 - 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 x ^ k / k - 1

d k - 1 = 1 - l 1 - l k ( 0 < l < 1 )

R ^ k = ( 1 - d k - 1 ) 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

x ^ k = x ^ k / k - 1 + K k ( Z k - H k 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: X . = f ( X ) + GW , System noise vector W = w gx b w gy b w gz b w ax b w ay b T

Factor arrays G = G 1 0 3 &times; 2 0 2 &times; 3 G 2 , G 1 = C 11 ' C 12 ' C 13 ' C 21 ' C 22 ' C 23 ' C 31 ' C 32 ' C 33 ' G 2 = C 11 ' C 21 ' C 21 ' C 22 ' ,

F (X) is

&phi; . x = - ( sin &phi; z ) &omega; ie cos L + &phi; y &omega; ie sin L - &delta; v y R M + C 11 ' ( &epsiv; x b + w gx b ) + C 12 ' ( &epsiv; y b + w gy b ) + C 13 ' ( &epsiv; z b + w gz b )

&phi; . y = ( 1 - cos &phi; z ) &omega; ie cos L - &phi; x &omega; ie sin L + &delta; v y R N + C 21 ' ( &epsiv; x b + w gx b ) + C 22 ' ( &epsiv; y b + w gy b ) + C 23 ' ( &epsiv; z b + w gz b )

&phi; . z = ( - &phi; y sin &phi; z + &phi; x cos &phi; z ) &omega; ie cos L + &delta; v x tan L R N + C 31 ' ( &epsiv; x b + w gx b ) + C 32 ' ( &epsiv; y b + w gy b ) + C 33 ' ( &epsiv; z b + w gz b ) &delta; v . x = - f x ( cos &phi; z - 1 ) + f y sin &phi; z - f z ( &phi; y cos &phi; z + &phi; x sin &phi; z ) + 2 &omega; ie &delta; v y sin L + C 11 ' ( &dtri; x b + w ax b ) + C 12 ' ( &dtri; y b + w ay b )

&delta; v . y = - f x sin &phi; z - f y ( cos &phi; z - 1 ) - f z ( &phi; y sin &phi; z - &phi; x cos &phi; z ) - 2 &omega; ie &delta; v x sin L + C 21 ' ( &dtri; x b + w ax b ) + C 22 ' ( &dtri; y b + w ay b )

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 × 20 2 × 3], I 2 × 2for unit second-order matrix, 0 2 × 3be 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 0, γ 0, 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

x ^ 0 = E [ x 0 ] , P 0 = E [ ( x 0 - x ^ 0 ) ( x 0 - x ^ 0 ) T ]

Wherein x 0for the initial value of state variable, P 0for 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 = ( h 2 - n ) / h 2 ( i = 0 ) 1 / ( 2 h 2 ) ( i = 1,2 , . . . , 2 n )

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

&xi; k - 1 = [ x ^ k - 1 , x ^ k - 1 + h P k - 1 , x ^ k - 1 + h 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-1k-1);

x ^ k / k - 1 = &Sigma; i = 0 2 n w i &gamma; i , k / k - 1 ;

P k / k - 1 = &Sigma; i = 0 2 n w i ( &gamma; i , k / k - 1 - x ^ k / k - 1 ) ( &gamma; i , k / k - 1 - 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 x ^ k / k - 1

d k - 1 = 1 - b 1 - b k ( 0 < b < 1 )

R ^ k = ( 1 - d k - 1 ) 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

x ^ k = x ^ k / k - 1 + K k ( Z k - H k 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 0, γ 0, 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 = w gx b w gy b w gz b w ax b w ay b T

Factor arrays G = G 1 0 3 &times; 2 0 2 &times; 3 G 2 , G 1 = C 11 ' C 12 ' C 13 ' C 21 ' C 22 ' C 23 ' C 31 ' C 32 ' C 33 ' G 2 = C 11 ' C 21 ' C 21 ' C 22 ' , F (X) is

&phi; . x = - ( sin &phi; z ) &omega; ie cos L + &phi; y &omega; ie sin L - &delta; v y R M + C 11 ' ( &epsiv; x b + w gx b ) + C 12 ' ( &epsiv; y b + w gy b ) + C 13 ' ( &epsiv; z b + w gz b )

&phi; . y = ( 1 - cos &phi; z ) &omega; ie cos L - &phi; x &omega; ie sin L + &delta; v y R N + C 21 ' ( &epsiv; x b + w gx b ) + C 22 ' ( &epsiv; y b + w gy b ) + C 23 ' ( &epsiv; z b + w gz b )

&phi; . z = ( - &phi; y sin &phi; z + &phi; x cos &phi; z ) &omega; ie cos L + &delta; v x tan L R N + C 31 ' ( &epsiv; x b + w gx b ) + C 32 ' ( &epsiv; y b + w gy b ) + C 33 ' ( &epsiv; z b + w gz b )

&delta; v . x = - f x ( cos &phi; z - 1 ) + f y sin &phi; z - f z ( &phi; y cos &phi; z + &phi; x sin &phi; z ) + 2 &omega; ie &delta; v y sin L + C 11 ' ( &dtri; x b + w ax b ) + C 12 ' ( &dtri; y b + w ay b )

&delta; v . y = - f x sin &phi; z - f y ( cos &phi; z - 1 ) - f z ( &phi; y sin &phi; z - &phi; x cos &phi; z ) - 2 &omega; ie &delta; v x sin L + C 21 ' ( &dtri; x b + w ax b ) + C 22 ' ( &dtri; y b + w ay b )

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 × 20 2 × 3] (I 2 × 2for unit second-order matrix, 0 2 × 3be 2 row 3 row null matrix); V is measurement noise;

4, filtering initial value is chosen

x ^ 0 = E [ x 0 ] , P 0 = E [ ( x 0 - x ^ 0 ) ( x 0 - x ^ 0 ) T ]

Wherein x 0for the initial value of state variable, P 0for 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 = ( h 2 - n ) / h 2 ( i = 0 ) 1 / ( 2 h 2 ) ( i = 1,2 , . . . , 2 n )

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

&xi; k - 1 = [ x ^ k - 1 , x ^ k - 1 + h P k - 1 , x ^ k - 1 + h 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-1k-1)

Status predication x ^ k / k - 1 = &Sigma; i = 0 2 n w i &gamma; i , k / k - 1

Predicated error P k / k - 1 = &Sigma; i = 0 2 n w i ( &gamma; i , k / k - 1 - x ^ k / k - 1 ) ( &gamma; i , k / k - 1 - 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 x ^ k / k - 1

Calculate weighting coefficient d k - 1 = 1 - b 1 - b k ( 0 < b < 1 )

The On-line Estimation of measurement noise R ^ k = ( 1 - d k - 1 ) 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 x ^ k = x ^ k / k - 1 + K k ( Z k - H k 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 0, gravity acceleration g 0=9.78049 (m/s 2), 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
x ^ 0 = E [ x 0 ] , P 0 = E [ ( x 0 - x ^ 0 ) ( x 0 - x ^ 0 ) T ]
Wherein x 0for the initial value of state variable, P 0for 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
θ 0pitch angle, γ 0roll 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 = ( h 2 - m ) / h 2 ( i = 0 ) 1 / ( 2 h 2 ) ( i = 1,2 , . . . , 2 m )
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:
&xi; k - 1 = [ x ^ k - 1 , x ^ k - 1 + h P k - 1 , x ^ k - 1 + h 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-1k-1);
x ^ k / k - 1 = &Sigma; i = 0 2 n w i &gamma; i , k / k - 1 ;
P k / k - 1 = &Sigma; i = 0 2 n w i ( &gamma; i , k / k - 1 - x ^ k / k - 1 ) ( &gamma; i , k / k - 1 - 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 x ^ k / k - 1
d k - 1 = 1 - l 1 - l k ( 0 < l < 1 )
R ^ k = ( 1 - d k - 1 ) R ^ k - 1 + d k - 1 ( e k e k T - HP k / k - 1 H T )
K k=P k/k-1H T(HP k/k-1H T+R k) -1
x ^ k = x ^ k / k - 1 + K k ( Z k - H k 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:
X . = f ( X ) + GW , System noise vector W = w gx b w gy b w gz b w ax b w ay b T
Factor arrays G = G 1 0 3 &times; 2 0 2 &times; 3 G 2 , G 1 = 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; G 2 = C 11 &prime; C 12 &prime; C 21 &prime; C 22 &prime; ,
F (X) is
&phi; . x = - ( sin &phi; z ) &omega; ie cos L + &phi; y &omega; ie sin L - &delta;v y R M + C 11 &prime; ( &epsiv; x b + w gx b ) + C 12 &prime; ( &epsiv; y b + w gy b ) + C 13 &prime; ( &epsiv; z b + w gz b )
&phi; . y = ( 1 - cos &phi; z ) &omega; ie cos L - &phi; x &omega; ie sin L + &delta; v x R N + C 21 &prime; ( &epsiv; x b + w gx b ) + C 22 &prime; ( &epsiv; y b + w gy b ) + C 23 &prime; ( &epsiv; z b + w gz b )
&phi; . z = ( - &phi; y sin &phi; z + &phi; x cos &phi; z ) &omega; ie cos L + &delta;v x tan L R N + C 31 &prime; ( &epsiv; x b + w gx b ) + C 32 &prime; ( &epsiv; y b + w gy b ) + C 33 &prime; ( &epsiv; z b + w gz b )
&delta; v . x = - f x ( cos &phi; z - 1 ) + f y sin &phi; z - f z ( &phi; y cos &phi; z + &phi; x sin &phi; z ) + 2 &omega; ie &delta; v y sin L + C 11 &prime; ( &dtri; x b + w ax b ) + C 12 &prime; ( &dtri; y b + w ay b )
&delta; v . y = - f x sin &phi; z - f y ( cos &phi; z - 1 ) - f z ( &phi; y sin &phi; z - &phi; x cos &phi; z ) - 2 &omega; ie &delta; v x sin L + C 21 &prime; ( &dtri; x b + w ax b ) + C 22 &prime; ( &dtri; y b + w ay b )
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 × 20 2 × 3], I 2 × 2for unit second-order matrix, 0 2 × 3be 2 row 3 row null matrix; V is measurement noise.
CN201410624222.0A 2014-11-06 2014-11-06 MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering CN104374405A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410624222.0A CN104374405A (en) 2014-11-06 2014-11-06 MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410624222.0A CN104374405A (en) 2014-11-06 2014-11-06 MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering

Publications (1)

Publication Number Publication Date
CN104374405A true CN104374405A (en) 2015-02-25

Family

ID=52553459

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410624222.0A CN104374405A (en) 2014-11-06 2014-11-06 MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering

Country Status (1)

Country Link
CN (1) CN104374405A (en)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104807479A (en) * 2015-05-20 2015-07-29 江苏华豪航海电器有限公司 Inertial navigation alignment performance evaluation method based on main inertial navigation attitude variation quantity assistance
CN105806363A (en) * 2015-11-16 2016-07-27 东南大学 Alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter)
CN106595705A (en) * 2016-11-22 2017-04-26 北京航天自动控制研究所 GPS-based flight inertial initial reference error estimation method
CN107389099A (en) * 2017-09-13 2017-11-24 哈尔滨工业大学 The aerial fast alignment device of SINS and method
CN107607977A (en) * 2017-08-22 2018-01-19 哈尔滨工程大学 A kind of adaptive UKF Combinated navigation methods based on the sampling of minimum degree of bias simple form
CN108955671A (en) * 2018-04-25 2018-12-07 珠海全志科技股份有限公司 A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103344260A (en) * 2013-07-18 2013-10-09 哈尔滨工程大学 Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter)
CN103557871A (en) * 2013-10-22 2014-02-05 北京航空航天大学 Strap-down inertial navigation air initial alignment method for floating aircraft
CN103575298A (en) * 2013-11-14 2014-02-12 哈尔滨工程大学 Self-regulation-based unscented Kalman filter (UKF) misalignment angle initial-alignment method

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103344260A (en) * 2013-07-18 2013-10-09 哈尔滨工程大学 Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter)
CN103557871A (en) * 2013-10-22 2014-02-05 北京航空航天大学 Strap-down inertial navigation air initial alignment method for floating aircraft
CN103575298A (en) * 2013-11-14 2014-02-12 哈尔滨工程大学 Self-regulation-based unscented Kalman filter (UKF) misalignment angle initial-alignment method

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
张斯明: "基于MEMS的捷联惯导系统组合对准技术研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 *
郝燕玲 等: "ACDKF在SINS大方位失准角初始对准中的应用", 《华中科技大学学报(自然科学版)》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104807479A (en) * 2015-05-20 2015-07-29 江苏华豪航海电器有限公司 Inertial navigation alignment performance evaluation method based on main inertial navigation attitude variation quantity assistance
CN105806363A (en) * 2015-11-16 2016-07-27 东南大学 Alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter)
CN105806363B (en) * 2015-11-16 2018-08-21 东南大学 The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN106595705A (en) * 2016-11-22 2017-04-26 北京航天自动控制研究所 GPS-based flight inertial initial reference error estimation method
CN106595705B (en) * 2016-11-22 2019-12-20 北京航天自动控制研究所 Method for estimating initial reference deviation of inertia in flight based on GPS
CN107607977A (en) * 2017-08-22 2018-01-19 哈尔滨工程大学 A kind of adaptive UKF Combinated navigation methods based on the sampling of minimum degree of bias simple form
CN107389099A (en) * 2017-09-13 2017-11-24 哈尔滨工业大学 The aerial fast alignment device of SINS and method
CN107389099B (en) * 2017-09-13 2019-11-12 哈尔滨工业大学 The aerial fast alignment device of Strapdown Inertial Navigation System and method
CN108955671A (en) * 2018-04-25 2018-12-07 珠海全志科技股份有限公司 A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle

Similar Documents

Publication Publication Date Title
Wu et al. Fast complementary filter for attitude estimation using low-cost MARG sensors
US9541392B2 (en) Surveying system and method
Wu et al. Optimization-based alignment for inertial navigation systems: Theory and algorithm
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment
Jiancheng et al. Study on innovation adaptive EKF for in-flight alignment of airborne POS
Li et al. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems
US9026263B2 (en) Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis
CN102829777B (en) Autonomous underwater vehicle combined navigation system and method
Syed et al. A new multi-position calibration method for MEMS inertial navigation systems
Gebre-Egziabher et al. Calibration of strapdown magnetometers in magnetic field domain
CN103759730B (en) The collaborative navigation system of a kind of pedestrian based on navigation information two-way fusion and intelligent mobile carrier and air navigation aid thereof
CN102445200B (en) Microminiature personal combined navigation system as well as navigating and positioning method thereof
US6459990B1 (en) Self-contained positioning method and system thereof for water and land vehicles
Groves Navigation using inertial sensors [Tutorial]
EP1642089B1 (en) Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients
US20200116753A1 (en) Multi sensor position and orientation measurement system
CN103630137B (en) A kind of for the attitude of navigational system and the bearing calibration of course angle
CN104075715B (en) A kind of underwater navigation localization method of Combining with terrain and environmental characteristic
Georgy et al. Low-cost three-dimensional navigation solution for RISS/GPS integration using mixture particle filter
CN103076017B (en) Method for designing Mars entry phase autonomous navigation scheme based on observability degree analysis
CN104061934A (en) Pedestrian indoor position tracking method based on inertial sensor
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
CN103471616B (en) Initial Alignment Method under a kind of moving base SINS Large azimuth angle condition
Han et al. A novel method to integrate IMU and magnetometers in attitude and heading reference systems
Han et al. A novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applications

Legal Events

Date Code Title Description
PB01 Publication
C06 Publication
SE01 Entry into force of request for substantive examination
C10 Entry into substantive examination
CB03 Change of inventor or designer information

Inventor after: He Kunpeng

Inventor after: Xu Xu

Inventor after: Wang Xiaoxue

Inventor after: Wang Gang

Inventor after: Wang Chenyang

Inventor after: Han Jitao

Inventor after: Shao Yuping

Inventor after: Zhang Xingzhi

Inventor after: Zhang Lin

Inventor after: Hu Pu

Inventor before: Wang Tongda

Inventor before: Song Chunyu

Inventor before: Li Meiling

Inventor before: Xu Yingjiao

Inventor before: Liu Ping

Inventor before: Yu Tianqi

COR Change of bibliographic data
RJ01 Rejection of invention patent application after publication

Application publication date: 20150225

RJ01 Rejection of invention patent application after publication