CN103344260A - Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter) - Google Patents

Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter) Download PDF

Info

Publication number
CN103344260A
CN103344260A CN2013103027603A CN201310302760A CN103344260A CN 103344260 A CN103344260 A CN 103344260A CN 2013103027603 A CN2013103027603 A CN 2013103027603A CN 201310302760 A CN201310302760 A CN 201310302760A CN 103344260 A CN103344260 A CN 103344260A
Authority
CN
China
Prior art keywords
initial alignment
rbckf
chi
covariance matrix
state
Prior art date
Application number
CN2013103027603A
Other languages
Chinese (zh)
Other versions
CN103344260B (en
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 CN201310302760.3A priority Critical patent/CN103344260B/en
Publication of CN103344260A publication Critical patent/CN103344260A/en
Application granted granted Critical
Publication of CN103344260B publication Critical patent/CN103344260B/en

Links

Abstract

The invention discloses an initial alignment method of a large azimuth misalignment angle of a strapdown inertial navigation system based on an RBCKF (rao-black-wellised cubature kalman filter), relates to the initial alignment method of the large azimuth misalignment angle of the strapdown inertial navigation system, and aims at solving the problems that the accuracy of the filtering method is low and easily diffused when the nonlinearity of the system is strong, and even EKF (exend kalman filter) filtering cannot be applied when the system is not continuous. The method comprises the following steps of: 1, building an error model for initial alignment of the large azimuth misalignment angle; 2, selecting an initial filtering value; 3, calculating a Cubature point set; 4, updating the time of a state variable and a measurement stable; and 5, updating a measurement equation. The initial alignment method is applied to the field of initial alignment of strapdown inertial navigation under the large azimuth misalignment angle.

Description

Strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF

Technical field

The present invention relates to strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method.

Background technology

The purpose of filtering (being state estimation) is according to certain method and criterion, goes a process of estimated state vector by the measurement amount that contains noise.Classic card Kalman Filtering from the beginning is various forms of nonlinear filterings till now, and through the development of nearly decades, filtering technique has become the focus of all trades and professions research.For filtering, stability, precision and speed of convergence are the problems that people generally are concerned about.

Strapdown inertial navitation system (SINS) (English full name: Strapdown inertial navigation system, be called for short SINS), inertial navigation mainly is by utilizing inertial sensor spare (comprising gyroscope and accelerometer) to measure carrier with respect to angular motion and the line motion of inertial space, and according to known starting condition, utilize machine solution to calculate navigational parameters such as the current velocity information of carrier, positional information and attitude information, further realize the navigational duty of carrier expection.

And for strapdown inertial navigation system, all inertial measurement component all directly are fixed on the carrier, the output data of inertial measurement component are angular velocity and the acceleration in carrier relative inertness space, be transformed under the navigation coordinate system by computing machine again and carry out navigation calculation, this process is equivalent to virtual platform in computing machine, to be used as the reference of navigation calculation.Stable platform in the effect of this virtual platform and the platform inertial navigation system is similar, and we are referred to as " mathematical platform ".For SINS itself, the precision of initial alignment and speed play a part very important for the SINS navigation procedure.Because the model of SINS initial alignment itself has nonlinear feature, therefore, adopt rational non-linear filtering method according to concrete requirement.

Use on the engineering the earliest and the most widely non-linear filtering method surely belong to EKF (Extend Kalman Filter, EKF).Its core concept is carried out linearization to nonlinear model at point of fixity exactly.But when system non-linear strong, this filtering method precision is lower, and disperses easily.Even when system was discontinuous, EKF filtering just can't be used.Based on the viewpoint of " be similar to probability distribution will to compare nonlinear function approximate a lot of easily ", Julier and Uhlmann proposed Unscented kalman filtering (Unscented Kalman Filter, UKF).It approaches through obeying average and the variance of gauss' condition distribution variable by deterministic Sigma point.(Cubature Kalman Filter CKF) is at first put forward by Ienkaran Arasaratnam and Simon Haykin the volume Kalman filtering.Its core concept is the Cubature conversion, namely waits the Cubature point of weights the variable of the Gaussian distributed after the nonlinear transformation to be carried out the calculating of average and variance by choosing 2n.CKF filtering is to derive according to Spherical-Radical Cubature criterion, compare with UKF have better stability, higher precision.From filtering method, CKF is a kind of special circumstances of UKF.But adopting of CKF counted less, calculated amount is little, and filtering accuracy also is better than UKF.The Cubature conversion that head and the tail are linked to each other in turn is applied to and has just produced CKF filtering on the Gaussian filter, and CKF filtering is all to have at state equation and measurement equation to propose when non-linear at first.

Summary of the invention

The present invention is when wanting resolution system non-linear strong, the filtering method precision is lower, and disperses easily, even when system is discontinuous, the problem that EKF filtering just can't be used, and strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF is provided.

Strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF realizes according to the following steps:

Step 1, according to the error characteristics of strapdown inertial navitation system (SINS), set up the error model of Initial Alignment of Large Azimuth Misalignment On, i.e. state equation and measurement equation;

Step 2, choose the filtering initial value:

Order With Wherein, x 0Be the initial value of state variable, P 0Initial error covariance matrix for state variable;

Step 3, according to the average of the state variable of current time With state error covariance matrix P K|k-1Calculate the Cubature point set: χ i = [ x ^ k - 1 | k - 1 , x ^ k - 1 | k - 1 + n * P k - 1 | k - 1 , x ^ k - 1 | k - 1 - n * P k - 1 | k - 1 ] ; Wherein, the average of described state variable With state error covariance matrix P K|k-1Calculate according to traditional CKF filtering method;

Wherein, Be the one-step prediction of the k-1 moment to k moment state variable average, P K|k-1Be the one-step prediction of the k-1 moment to k moment state error covariance matrix; And P K|k-1Calculating all add up through state equation nonlinear propagation rear weight by k-1 Cubature point constantly and obtain, the weights of each point are 1/2n, n is the dimension of system state variables;

Step 4, under state equation is non-linear, that measurement equation is linear condition, the time of carrying out state variable and measuring variable upgrades;

χ i , k | k - 1 * = f ( χ i , k - 1 ) x ^ k | k - 1 = Σ i = 1 2 n w i m * χ i , k | k - 1 * z ^ k | k - 1 = H k * x ^ k | k - 1

Step 5, renewal error covariance matrix

P x ^ k z ^ k = P k | k - 1 H k T

P k | k - 1 = Σ i = 1 2 n w i c ( χ i , k | k - 1 * - x ^ k | k - 1 ) ( χ i , k | k - 1 * - x ^ k | k - 1 ) T + Q k

P z ^ k z ^ k = H k P k | k - 1 H k T + R k

Make k=k+1, continue to carry out from step 3, namely finished the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF.

The invention effect:

Concrete error model at the big misalignment initial alignment of SINS has proposed a kind of filtering method, namely marginalisation volume Kalman filtering (Rao-Black-wellised Cubature Kalman Filter, RBCKF).When state equation and measurement equation all have when non-linear, traditional CKF is a kind of very outstanding non-linear filtering method.But the state equation of the big misalignment initial alignment of SINS has non-linear, and measurement equation has linear characteristics, therefore can CKF be improved, and namely state equation is adopted the Cubature conversion, and measurement equation is taked linear the renewal.Owing to no matter carry out Cubature conversion or linear transformation for linear model, the precision after the conversion is identical.Therefore, RBCKF can reduce the time of SINS initial alignment when not losing filtering accuracy.

The basis of CKF filtering is the Cubature conversion, and this conversion can be regarded the Nonlinear Mapping of different dimension linear space as.It approaches average and the variance of variable after the conversion by taking some fixed points.Because SINS is when Initial Alignment of Large Azimuth Misalignment On, state equation has non-linear and measurement equation has linear characteristics.Therefore, can carry out certain improvement to CKF, make under the constant condition of filtering accuracy, shorten the time of aiming at.

One, the present invention compares traditional CKF and does not lose precision of filtering;

Two, technology of the present invention is compared the time that CKF can shorten SINS initial alignment under big orientation misalignment;

Three, to go for any state equation be non-linear to technology of the present invention and measurement equation is linear Filtering Model.

Description of drawings

Fig. 1 is process flow diagram of the present invention;

Fig. 2 is the schematic diagram of RBCKF in the embodiment one;

Fig. 3 is the schematic diagram of SINS initial alignment in the embodiment one;

Fig. 4 is the rolling misalignment graph of errors in the l-G simulation test; Wherein,---expression CKF estimates that-----expression RBCKF estimates;

Fig. 5 is the pitching misalignment graph of errors in the l-G simulation test; Wherein,---expression CKF estimates that-----expression RBCKF estimates;

Fig. 6 is the course misalignment graph of errors in the l-G simulation test; Wherein,---expression CKF estimates that-----expression RBCKF estimates;

Fig. 7 is the course misalignment steady-state error curve in the l-G simulation test; Wherein,---expression CKF estimates that-----expression RBCKF estimates.

Embodiment

Embodiment one: the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF of present embodiment realizes according to the following steps:

Step 1, according to the error characteristics of strapdown inertial navitation system (SINS), set up the error model of Initial Alignment of Large Azimuth Misalignment On, i.e. state equation and measurement equation;

Step 2, choose the filtering initial value:

Order With Wherein, x 0Be the initial value of state variable, P 0Initial error covariance matrix for state variable;

Step 3, according to the average of the state variable of current time With state error covariance matrix P K|k-1Calculate the Cubature point set: χ i = [ x ^ k - 1 | k - 1 , x ^ k - 1 | k - 1 + n * P k - 1 | k - 1 , x ^ k - 1 | k - 1 n * P k - 1 | k - 1 ] ; Wherein, the average of described state variable With state error covariance matrix P K|k-1Calculate according to traditional CKF filtering method;

Wherein, Be the one-step prediction of the k-1 moment to k moment state variable average, P K|k-1Be the one-step prediction of the k-1 moment to k moment state error covariance matrix; And P K|k-1Calculating all add up through state equation nonlinear propagation rear weight by k-1 Cubature point constantly and obtain, the weights of each point are 1/2n, n is the dimension of system state variables;

Step 4, under state equation is non-linear, that measurement equation is linear condition, the time of carrying out state variable and measuring variable upgrades;

χ i , k | k - 1 * = f ( χ i , k - 1 ) x ^ k | k - 1 = Σ i = 1 2 n w i m * χ i , k | k - 1 * z ^ k | k - 1 = H k * x ^ k | k - 1

Step 5, renewal error covariance matrix

P x ^ k z ^ k = P k | k - 1 H k T

P k | k - 1 = Σ i = 1 2 n w i c ( χ i , k | k - 1 * - x ^ k | k - 1 ) ( χ i , k | k - 1 * - x ^ k | k - 1 ) T + Q k

P z ^ k z ^ k = H k P k | k - 1 H k T + R k

Make k=k+1, continue to carry out from step 3, namely finished the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF.

In the present embodiment, in the described step 2, because the state-space expression of SINS Initial Alignment of Large Azimuth Misalignment On is fully controlled and observable, and system can regard uniformly stable as, therefore, choosing of filtering starting condition generally can not influence the precision that filtering restrains;

In the step 5, owing to for linear equation, no matter be Cubature conversion or linear transformation, the precision after the conversion is identical.Therefore, RBCKF is identical with the filtering accuracy of CKF.But on calculated amount, when calculating measurement average, variance and simple crosscorrelation covariance, RBCKF does not but adopt 2n Cubature point again and carries out computings such as nonlinear transformation and square root decomposition, thereby has saved running time of algorithm.Simultaneously, from χ i , k | k - 1 * = f ( χ i , k - 1 ) x ^ k | k - 1 = Σ i = 1 2 n w i m * χ i , k | k - 1 * z ^ k | k - 1 = H k * x ^ k | k - 1 The 3rd equation, Formula and Find out in the formula that RBCKF has carried out nm altogether 2+ 2n 2M+mn multiplication and m 2+ nm 2+ 2n 2The m+mn sub-addition.And by contrast, CKF not only will produce the Cubature point again, and the nonlinear transformation that measures also will be carried out 2mn+4nm simultaneously 2+ 4n 2M multiplication and m 2+ 2nm 2+ 2n 2M+8nm+2n 2Sub-addition.As can be seen, along with the increase of m and n, the calculated amount of RBCKF will be far smaller than the calculated amount of CKF.

As seen, the RBCKF filtering algorithm is applied in the SINS Initial Alignment of Large Azimuth Misalignment On, can when not losing filtering accuracy, shortens the time of aiming at greatly.

The present embodiment effect:

Concrete error model at the big misalignment initial alignment of SINS has proposed a kind of filtering method, namely marginalisation volume Kalman filtering (Rao-Black-wellised Cubature Kalman Filter, RBCKF).When state equation and measurement equation all have when non-linear, traditional CKF is a kind of very outstanding non-linear filtering method.But the state equation of the big misalignment initial alignment of SINS has non-linear, and measurement equation has linear characteristics, therefore can CKF be improved, and namely state equation is adopted the Cubature conversion, and measurement equation is taked linear the renewal.Owing to no matter carry out Cubature conversion or linear transformation for linear model, the precision after the conversion is identical.Therefore, RBCKF can reduce the time of SINS initial alignment when not losing filtering accuracy.

The basis of CKF filtering is the Cubature conversion, and this conversion can be regarded the Nonlinear Mapping of different dimension linear space as.It approaches average and the variance of variable after the conversion by taking some fixed points.Because SINS is when Initial Alignment of Large Azimuth Misalignment On, state equation has non-linear and measurement equation has linear characteristics.Therefore, can carry out certain improvement to CKF, make under the constant condition of filtering accuracy, shorten the time of aiming at.

One, present embodiment is compared traditional CKF and is not lost precision of filtering;

Two, the technology of present embodiment is compared the time that CKF can shorten SINS initial alignment under big orientation misalignment;

Three, to go for any state equation be non-linear to the technology of present embodiment and measurement equation is linear Filtering Model.

Embodiment two: what present embodiment and embodiment one were different is: the error model of setting up Initial Alignment of Large Azimuth Misalignment On in the step 1 is specially:

Choose the velocity error of east orientation, north orientation in the general quiet pedestal Initial Alignment of Large Azimuth Misalignment On And the misalignment φ in three orientation x, φ yAnd φ zBe state variable, namely Therefore, the state equation of its quiet pedestal initial alignment is as follows:

Wherein, Be local latitude; ω IeBe rotational-angular velocity of the earth; With Be three axial gyroscopic drifts; With Be that three axial acceleration zero are inclined to one side; f x, f yAnd f zFor the ratio output valve of accelerometer is being calculated projection in the Department of Geography; C IjBe that carrier is tied to the element in the direction matrix of Department of Geography; C ' IjBe that carrier is tied to the element that calculates in Department of Geography's matrix;

Choose east orientation and north orientation velocity error Be semblance measure, the measurement equation of strapdown inertial navitation system (SINS) initial alignment is: Z=HX+V

Wherein, H gets H=[I 2 * 20 2 * 3]; V is measurement noise.Other step and parameter are identical with embodiment one.

Embodiment three: what present embodiment was different with embodiment one or two is: calculate the Cubature point set in the step 3 and be specially:

The error covariance matrix is carried out square root decompose, choose the Cubature point set then, namely χ i = [ x ^ k - 1 | k - 1 , x ^ k - 1 | k - 1 + n * P k - 1 | k - 1 , x ^ k - 1 | k - 1 - n * P k - 1 | k - 1 ] . Other step and parameter are identical with embodiment one or two.

Embodiment four: what present embodiment was different with one of embodiment one to three is: the time renewal of carrying out state variable and measurement variable in the step 4 is specially: in the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On, state equation is non-linear and measurement equation is linear, and concrete mathematic(al) representation is as follows:

X . = f ( X ) + GW Z = HX + V

Time for state variable upgrades, and will calculate the one-step prediction average of state variable after the process of the Cubature point set in the step 3 nonlinear transformation of state equation , and the time of measurement amount upgrades not through the Cubature conversion but directly carries out the one-step prediction average that the linear transformation calculated amount is measured , detailed process is as shown in the formula calculating:

χ i , k | k - 1 * = f ( χ i , k - 1 ) x ^ k | k - 1 = Σ i = 1 2 n w i m * χ i , k | k - 1 * z ^ k | k - 1 = H k * x ^ k | k - 1 . Other step and parameter are identical with one of embodiment one to three.

Embodiment five: what present embodiment was different with one of embodiment one to four is: step 5 is upgraded the error covariance matrix and is specially:

When the measurement of carrying out filtering is upgraded, need the error covariance matrix P of computing mode variable K|k-1, the error covariance matrix of measurement variable And simple crosscorrelation covariance matrix Then these variablees are applied on the Gaussian filter, have just formed the RBCKF filtering algorithm;

Step May Day: in RBCKF, the error covariance matrix P of state variable K|k-1Calculating identical with the computing method of traditional CKF, still need to calculate by the Cubature point set, and will include the influence of state-noise in, concrete meter

Calculate formula as P k | k - 1 = Σ i = 1 2 n w i c ( χ i , k | k - 1 * - x ^ k | k - 1 ) ( χ i , k | k - 1 * - x ^ k | k - 1 ) T + Q k Formula;

Measure the error covariance matrix of variable And simple crosscorrelation covariance matrix Calculating different with traditional CKF, Be directly to upgrade by linear equation, Need carry out the fusion of nonlinear equation and linear equation, it is as follows specifically to derive:

P z ^ k z ^ k = E [ ( H k x ^ k | k - 1 - H k x k - v k ) ( H k x ^ k | k - 1 - H k x k - v k ) T ]

= H k E [ ( x ^ k | k - 1 - x k ) ( x ^ k | k - 1 - x k ) T ] H k T + E [ v k v k T ]

= H k P k | k - 1 H k T + R k

P x ^ k z ^ k = E [ ( x ^ k | k - 1 - x k ) ( H k x ^ k | k - 1 - H k x k - v k ) T ]

= E [ ( x ^ k | k - 1 - x k ) ( x ^ k | k - 1 - x k ) T ] H k T + E [ ( x ^ k | k - 1 - x k ) v k T ]

= E [ ( x ^ k | k - 1 - x k ) ( x ^ k | k - 1 - x k ) T ] H k T + E [ ( x ^ k | k - 1 - x k ) ] * E [ v k T ]

= E [ ( x ^ k | k - 1 - x k ) ( x ^ k | k - 1 - x k ) T ] H k T = P k | k - 1 H k T

Step 5 two: will And Be re-applied under the Gaussian filter, just produced RBCKF, the general type of Gaussian filter is as follows:

x k | k - 1 = x ^ k | k - 1 + W k ( z k - z ^ k ) P k | k = P k | k - 1 - W k P z ^ k z ^ k W k T W k = P x ^ k z ^ k P z ^ k z ^ k - 1

Step 5 three: compare CKF, RBCKF can shorten the convergence time of filtering greatly when not losing filtering accuracy, specifically be compared as follows:

Suppose that measurement equation is: Z k=H kX k+ V k(wherein, H kMatrix for m * n) from whole filtering algorithm, RBCKF and CKF are just calculating the measurement average Measure variance And simple crosscorrelation covariance matrix The time exist difference, in traditional C KF, this computing formula of three is as follows:

χ k | k - 1 = x ^ k | k - 1 x ^ k | k - 1 + n * P k | k - 1 x ^ k | k - 1 - n - P k | k - 1 η i , k | k - 1 = h ( χ i , k | k - 1 ) z ^ k | k - 1 = Σ i = 1 2 n w i ( m ) η i , k | k - 1

P z ^ k z ^ k = Σ i = 1 2 n w i ( c ) ( η i , k | k - 1 - z ^ k { k - 1 ) ( η i , k | k - 1 - z ^ k { k - 1 ) T + R k

P x ^ k z ^ k = Σ i = 1 2 n w i ( c ) ( χ i , k | k - 1 - x ^ k { k - 1 ) ( η i , k | k - 1 - z ^ k { k - 1 ) T .

Other step and parameter are identical with one of embodiment one to four.

Verify by following l-G simulation test:

Before the SINS initial alignment, choose the initial value x (0)=0 of state variable; Initial east orientation, north orientation and sky are set at respectively to misalignment: first group: φ EN=0.5 0, φ U=10 0Second group: φ EN=0.5 0, φ U=15 0The 3rd group: φ EN=0.5 0, φ U=20 0Calculate according to the medium accuracy gyro, the constant value drift of supposing gyro is 0.1 0/ h, random drift is 0.05 0/ h; The zero drift of accelerometer is 1.0 * 10 -4M/s 2, random deviation is 0.5 * 10 -4G;

Filtering parameter is set as follows:

The initial error covariance matrix of state variable x is P 0=diag{ (0.1m/s) 2(0.1m/s) 2(0.5 0) 2(0.5 0) 2(10 0) 2; The random drift of degree of will speed up meter random deviation and gyro is designated as process noise, and then the noise error covariance matrix of state equation is Q=diag{ (50ug) 2(50ug) 2(0.05 0/ h) 2(0.05 0/ h) 2(0.05 0/ h) 2; Simultaneously, the noise error covariance matrix of measurement equation is designated as R=diag{ (0.1m/s) 2(0.1m/s) 2.The filtering algorithm of CKF and RBCKF is as shown in table 1 working time under the different azimuth misalignment:

The operation time of CKF and RBCKF under the different initial orientation misalignments of table 1

Provided the working time of two kinds of filtering algorithms under the different big orientation misalignment in the table 1.Because measurement equation is linear, RBCKF has adopted the mode of linear renewal when upgrading in the time, compares the mode that 2n point adopted in traditional C KF filtering, and the calculated amount of RBCKF is little.Therefore, under identical orientation misalignment, the working time of RBCKF is always less than working time of traditional CKF filtering.That is to say that RBCKF can improve the alignment speed of inertial navigation initial alignment.

Fig. 4~7 have provided at φ xy=0.5 0, φ z=10 0The estimation misalignment graph of errors in following rolling, pitching and course.

Because it is wide-angle that initial misalignment has only the orientation misalignment, and horizontal misalignment is low-angle.Therefore, Fig. 7 has provided the steady-state error curve map of orientation misalignment error.

As can be seen from Figure 7 CKF is identical for course misalignment steady-state error value with RBCKF, is-3.588'.Therefore, the filtering accuracy of two kinds of filtering methods is identical.

To sum up, the RBCKF filtering algorithm is than traditional C KF filtering algorithm, and precision of filtering is identical, but from algorithm on working time, and the working time of RBCKF is always less than working time of traditional C KF under identical big orientation misalignment.Therefore, RBCKF can help to shorten the time that strapdown inertial navitation system (SINS) is aimed at when not losing filtering accuracy.

Claims (5)

1. based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF, it is characterized in that realizing according to the following steps based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF:
Step 1, according to the error characteristics of strapdown inertial navitation system (SINS), set up the error model of Initial Alignment of Large Azimuth Misalignment On, i.e. state equation and measurement equation;
Step 2, choose the filtering initial value:
Order x ^ 0 = E [ x 0 ] With P 0 = [ ( x 0 - x ^ 0 ) ( x 0 - x ^ 0 ) T ] ; Wherein, x 0Be the initial value of state variable, P 0Initial error covariance matrix for state variable;
Step 3, according to the average of the state variable of current time With state error covariance matrix P K|k-1Calculate the Cubature point set: χ i = [ x ^ k - 1 | k - 1 , x ^ k - 1 | k - 1 + n * P k - 1 | k - 1 , x ^ k - 1 | k - 1 - n * P k - 1 | k - 1 ] ; Wherein, the average of described state variable With state error covariance matrix P K|k-1Calculate according to traditional CKF filtering method;
Wherein, Be the one-step prediction of the k-1 moment to k moment state variable average, P K|k-1Be the one-step prediction of the k-1 moment to k moment state error covariance matrix; And P K|k-1Calculating all add up through state equation nonlinear propagation rear weight by k-1 Cubature point constantly and obtain, the weights of each point are 1/2n, n is the dimension of system state variables;
Step 4, under state equation is non-linear, that measurement equation is linear condition, the time of carrying out state variable and measuring variable upgrades;
χ i , k | k - 1 * = f ( χ i , k - 1 ) x ^ k | k - 1 = Σ i = 1 2 n w i m * χ i , k | k - 1 * z ^ k | k - 1 = H k * x ^ k | k - 1
Step 5, renewal error covariance matrix
P x ^ k z ^ k = P k - | k - 1 H k T
P k | k - 1 = Σ i = 1 2 n w i c ( χ i , k | k - 1 * - x ^ k | k - 1 ) ( χ i , k | k - 1 * - x ^ k | k - 1 ) T + Q k
P z ^ k z ^ k = H k P k | k - 1 H k T + R k
Make k=k+1, continue to carry out from step 3, namely finished the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF.
2. the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF according to claim 1 is characterized in that the error model of setting up Initial Alignment of Large Azimuth Misalignment On in the step 1 is specially:
Choose the velocity error δ v of east orientation, north orientation in the general quiet pedestal Initial Alignment of Large Azimuth Misalignment On x, δ v yAnd the misalignment φ in three orientation x, φ yAnd φ zBe state variable, i.e. X=[δ v xδ v yφ xφ yφ z] T, therefore, the state equation of its quiet pedestal initial alignment is as follows:
Wherein, Be local latitude; ω IeBe rotational-angular velocity of the earth; With Be three axial gyroscopic drifts; With Be that three axial acceleration zero are inclined to one side; f x, f yAnd f zFor the ratio output valve of accelerometer is being calculated projection in the Department of Geography; C IjBe that carrier is tied to the element in the direction matrix of Department of Geography; C IjBe that carrier is tied to the element that calculates in Department of Geography's matrix;
Choose east orientation and north orientation velocity error δ v x, δ v yBe semblance measure, the measurement equation of strapdown inertial navitation system (SINS) initial alignment is:
Z=HX+V
Wherein, H gets H=[I 2 * 20 2 * 3]; V is measurement noise.
3. the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF according to claim 1 is characterized in that calculating in the step 3 Cubature point set and is specially:
The error covariance matrix is carried out square root decompose, choose the Cubature point set then, namely χ i = [ x ^ k - 1 | k - 1 , x ^ k - 1 | k - 1 + n * P k - 1 | k - 1 , x ^ k - 1 | k - 1 - n * P k - 1 | k - 1 ] .
4. the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF according to claim 1, it is characterized in that the time renewal of carrying out state variable in the step 4 and measuring variable is specially: in the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On, state equation is non-linear and measurement equation is linear, and concrete mathematic(al) representation is as follows:
X • = f ( X ) + GW Z = HX + V
Time for state variable upgrades, and will calculate the one-step prediction average of state variable after the process of the Cubature point set in the step 3 nonlinear transformation of state equation And the time of measurement amount upgrades not through the Cubature conversion but directly carries out the one-step prediction average that the linear transformation calculated amount is measured Detailed process is as shown in the formula calculating:
χ i , k | k - 1 * = f ( χ i , k - 1 ) x ^ k | k - 1 = Σ i = 1 2 n w i m * χ i , k | k - 1 * z ^ k | k - 1 = H k * x ^ k | k - 1 .
5. the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF according to claim 1 is characterized in that step 5 upgrades the error covariance matrix and be specially:
When the measurement of carrying out filtering is upgraded, need the error covariance matrix P of computing mode variable K|k-1, the error covariance matrix of measurement variable And simple crosscorrelation covariance matrix Then these variablees are applied on the Gaussian filter, have just formed the RBCKF filtering algorithm;
Step May Day: in RBCKF, the error covariance matrix P of state variable K|k-1Calculating identical with the computing method of traditional CKF, still need to calculate by the Cubature point set, and will include the influence of state-noise in, concrete computing formula as P k | k - 1 = Σ i = 1 2 n w i c ( χ i , k | k - 1 * - x ^ k | k - 1 ) ( χ i , k | k - 1 * - x ^ k | k - 1 ) T + Q k Formula;
Measure the error covariance matrix of variable And simple crosscorrelation covariance matrix Calculating different with traditional CKF, Be directly to upgrade by linear equation, Need carry out the fusion of nonlinear equation and linear equation, it is as follows specifically to derive:
P z ^ k z ^ k = E [ ( H k x ^ k | k - 1 - H k x k - v k ) ( H k x ^ k | k - 1 - H k x k - v k ) T ]
= H k E [ ( x ^ k | k - 1 - x k ) ( x ^ k | k - 1 - x k ) T ] H k T + E [ v k v k T ]
= H k P k | k - 1 H k T + R k
P x ^ k z ^ k = E [ ( x ^ k | k - 1 - x k ) ( H k x ^ k | k - 1 - H k x k - v k ) T ]
= E [ ( x ^ k | k - 1 - x k ) ( x ^ k | k - 1 - x k ) T H k T ( x ^ k | k - 1 - x k ) v k T ]
= E [ ( x ^ k | k - 1 - x k ) ( x ^ k | k - 1 - x k ) T ] H k T + E [ ( x ^ k | k - 1 - x k ) ] * E [ v k T ]
= E [ ( x ^ k | k - 1 ) ( x ^ k | k - 1 - x k ) T ] H k T = P k | k - 1 H k T
Step 5 two: will P K|k-1, And Be re-applied under the Gaussian filter, just produced RBCKF, the general type of Gaussian filter is as follows:
x k | k = x ^ k | k - 1 + W k ( z k - z ^ k ) P k | k = P k | k - 1 - W k P z ^ k z ^ k W k T W k = P x ^ k z ^ k P z 6 k z ^ k
Step 5 three: compare CKF, RBCKF can shorten the convergence time of filtering greatly when not losing filtering accuracy, specifically be compared as follows:
Suppose that measurement equation is: Z k=H kX k+ V k(wherein, H kMatrix for m * n) from whole filtering algorithm, RBCKF and CKF are just calculating the measurement average Measure variance And simple crosscorrelation covariance matrix The time exist difference, in traditional C KF, this computing formula of three is as follows:
χ k | k - 1 = [ x ^ k | k - 1 x ^ k | k - 1 + n * P k | k - 1 x ^ k | k - 1 - n * P k | k - 1 ] η i , k | k - 1 = h ( χ i , k | k - 1 ) z ^ k | k - 1 = Σ i = 1 2 n w i ( m ) η i , k | k - 1
P z ^ k z ^ k = Σ i = 1 2 n w i ( c ) ( η i , k | k - 1 - z ^ k { k - 1 ) ( η i , k | k - 1 - z ^ k { k - 1 ) T + R k
P x ^ k z ^ k = Σ i = 1 2 n w i ( c ) ( χ i , k | k - 1 - x ^ k { k - 1 ) ( η i , k | k - 1 - z ^ k { k - 1 ) T .
CN201310302760.3A 2013-07-18 2013-07-18 Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF CN103344260B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201310302760.3A CN103344260B (en) 2013-07-18 2013-07-18 Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201310302760.3A CN103344260B (en) 2013-07-18 2013-07-18 Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF

Publications (2)

Publication Number Publication Date
CN103344260A true CN103344260A (en) 2013-10-09
CN103344260B CN103344260B (en) 2016-04-27

Family

ID=49279076

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201310302760.3A CN103344260B (en) 2013-07-18 2013-07-18 Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF

Country Status (1)

Country Link
CN (1) CN103344260B (en)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103575298A (en) * 2013-11-14 2014-02-12 哈尔滨工程大学 Self-regulation-based unscented Kalman filter (UKF) misalignment angle initial-alignment method
CN103727940A (en) * 2014-01-15 2014-04-16 东南大学 Gravity acceleration vector fitting-based nonlinear initial alignment method
CN103727941A (en) * 2014-01-06 2014-04-16 东南大学 Volume kalman nonlinear integrated navigation method based on carrier system speed matching
CN103791918A (en) * 2014-02-10 2014-05-14 哈尔滨工程大学 Polar region moving base alignment method for naval vessel strapdown inertial navigation system
CN104374401A (en) * 2014-10-15 2015-02-25 哈尔滨工程大学 Compensating method of gravity disturbance in strapdown inertial navigation initial alignment
CN104374405A (en) * 2014-11-06 2015-02-25 哈尔滨工程大学 MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering
CN104567871A (en) * 2015-01-12 2015-04-29 哈尔滨工程大学 Quaternion Kalman filtering attitude estimation method based on geomagnetic gradient tensor
CN105004351A (en) * 2015-05-14 2015-10-28 东南大学 SINS large-azimuth misalignment angle initial alignment method based on self-adaptation UPF
CN105180968A (en) * 2015-09-02 2015-12-23 北京天航华创科技股份有限公司 IMU/magnetometer installation misalignment angle online filter calibration method
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)
CN106123921A (en) * 2016-07-10 2016-11-16 北京工业大学 Latitude the unknown Alignment Method of SINS under the conditions of dynamic disturbance
CN106840194A (en) * 2016-09-20 2017-06-13 南京喂啊游通信科技有限公司 A kind of Large azimuth angle linear alignment method

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101915579A (en) * 2010-07-15 2010-12-15 哈尔滨工程大学 Novel CKF(Crankshaft Fluctuation Sensor)-based SINS (Ship Inertial Navigation System) large misalignment angle initially-aligning method

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101915579A (en) * 2010-07-15 2010-12-15 哈尔滨工程大学 Novel CKF(Crankshaft Fluctuation Sensor)-based SINS (Ship Inertial Navigation System) large misalignment angle initially-aligning method

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
ARNAUD DOUCET,ER AL.: "Particle Filters for State Estimation of Jump Markov Linear Systems", 《IEEE TRANSACTIONS ON SIGNAL PROCESSING》, vol. 49, no. 3, 31 March 2001 (2001-03-31), XP011059256 *
庄泽森等: "混合线性/非线性模型的准高斯Rao-Blackwellized粒子滤波法", 《航空学报》, vol. 29, no. 2, 31 March 2008 (2008-03-31) *

Cited By (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103575298A (en) * 2013-11-14 2014-02-12 哈尔滨工程大学 Self-regulation-based unscented Kalman filter (UKF) misalignment angle initial-alignment method
CN103727941A (en) * 2014-01-06 2014-04-16 东南大学 Volume kalman nonlinear integrated navigation method based on carrier system speed matching
CN103727940A (en) * 2014-01-15 2014-04-16 东南大学 Gravity acceleration vector fitting-based nonlinear initial alignment method
CN103727940B (en) * 2014-01-15 2016-05-04 东南大学 Nonlinear initial alignment method based on acceleration of gravity vector matching
CN103791918A (en) * 2014-02-10 2014-05-14 哈尔滨工程大学 Polar region moving base alignment method for naval vessel strapdown inertial navigation system
CN104374401A (en) * 2014-10-15 2015-02-25 哈尔滨工程大学 Compensating method of gravity disturbance in strapdown inertial navigation initial alignment
CN104374405A (en) * 2014-11-06 2015-02-25 哈尔滨工程大学 MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering
CN104567871B (en) * 2015-01-12 2018-07-24 哈尔滨工程大学 A kind of quaternary number Kalman filtering Attitude estimation method based on earth magnetism gradient tensor
CN104567871A (en) * 2015-01-12 2015-04-29 哈尔滨工程大学 Quaternion Kalman filtering attitude estimation method based on geomagnetic gradient tensor
CN105004351A (en) * 2015-05-14 2015-10-28 东南大学 SINS large-azimuth misalignment angle initial alignment method based on self-adaptation UPF
CN105180968A (en) * 2015-09-02 2015-12-23 北京天航华创科技股份有限公司 IMU/magnetometer installation misalignment angle online filter calibration method
CN105180968B (en) * 2015-09-02 2018-06-01 北京天航华创科技股份有限公司 A kind of IMU/ magnetometers installation misalignment filters scaling method online
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
CN106123921A (en) * 2016-07-10 2016-11-16 北京工业大学 Latitude the unknown Alignment Method of SINS under the conditions of dynamic disturbance
CN106123921B (en) * 2016-07-10 2019-05-24 北京工业大学 The unknown Alignment Method of the latitude of Strapdown Inertial Navigation System under the conditions of dynamic disturbance
CN106840194A (en) * 2016-09-20 2017-06-13 南京喂啊游通信科技有限公司 A kind of Large azimuth angle linear alignment method
CN106840194B (en) * 2016-09-20 2019-09-27 南京喂啊游通信科技有限公司 A kind of Large azimuth angle linear alignment method

Also Published As

Publication number Publication date
CN103344260B (en) 2016-04-27

Similar Documents

Publication Publication Date Title
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
Imsland et al. Vehicle velocity estimation using nonlinear observers
Zhang et al. A new method of seamless land navigation for GPS/INS integrated system
Georgy et al. Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter
Wu et al. Observability of strapdown INS alignment: A global perspective
Kong INS algorithm using quaternion model for low cost IMU
KR101209667B1 (en) Improved gps accumulated delta range processing for navigation processing
Wang Fast alignment and calibration algorithms for inertial navigation system
CN101246022B (en) Optic fiber gyroscope strapdown inertial navigation system two-position initial alignment method based on filtering
CN103471616B (en) Initial Alignment Method under a kind of moving base SINS Large azimuth angle condition
CN103076015B (en) A kind of SINS/CNS integrated navigation system based on optimum correction comprehensively and air navigation aid thereof
Vasconcelos et al. Embedded UAV model and LASER aiding techniques for inertial navigation systems
JP5071533B2 (en) Current position detection device for vehicle
CN102519460B (en) Non-linear alignment method of strapdown inertial navigation system
CN105043385A (en) Self-adaption Kalman filtering method for autonomous navigation positioning of pedestrians
CN100516775C (en) Method for determining initial status of strapdown inertial navigation system
Chen et al. A hybrid prediction method for bridging GPS outages in high-precision POS application
US8639441B2 (en) Vehicle navigation on the basis of satellite positioning data and vehicle sensor data
Cui et al. Improved cubature Kalman filter for GNSS/INS based on transformation of posterior sigma-points error
Li et al. A reliable fusion positioning strategy for land vehicles in GPS-denied environments based on low-cost sensors
CN101706284B (en) Method for increasing position precision of optical fiber gyro strap-down inertial navigation system used by ship
CN103063212B (en) A kind of Combinated navigation method based on nonlinear mapping ADAPTIVE MIXED Kalman/H ∞ wave filter
CN104655131B (en) Inertial navigation Initial Alignment Method based on ISTSSRCKF
CN102353378B (en) Adaptive federal filtering method of vector-form information distribution coefficients
CN101438184B (en) A kind of method of state of tracking mobile electronic equipment

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20160427

Termination date: 20180718