CN103344260B - Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF - Google Patents

Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF Download PDF

Info

Publication number
CN103344260B
CN103344260B CN201310302760.3A CN201310302760A CN103344260B CN 103344260 B CN103344260 B CN 103344260B CN 201310302760 A CN201310302760 A CN 201310302760A CN 103344260 B CN103344260 B CN 103344260B
Authority
CN
China
Prior art keywords
amp
rbckf
initial alignment
chi
sins
Prior art date
Application number
CN201310302760.3A
Other languages
Chinese (zh)
Other versions
CN103344260A (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

Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF, the present invention relates to strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method.The present invention is that when wanting resolution system non-linear stronger, filtering method precision is lower, and easily disperses, even when system is discontinuous, and the problem that EKF filtering just cannot be applied.One, the error model of Initial Alignment of Large Azimuth Misalignment On is set up; Two, filtering initial value is chosen; Three, Cubature point set is calculated; The time of four, carrying out state variable and measurement variable upgrades; Five, measurement equation is upgraded.The present invention is applied to the inertial navigation initial alignment field under Large azimuth angle.

Description

Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF

Technical field

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

Background technology

The object of filtering (i.e. state estimation) is according to certain method and criterion, by going a process of estimated state vector containing noisy measurement amount.The various forms of nonlinear filtering till now of classic card Kalman Filtering from the beginning, 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 of people's general concern.

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

And for strapdown inertial navigation system, all inertial measurement component are all directly fixed on carrier, the output data of inertial measurement component are angular velocity and the acceleration in carrier relative inertness space, navigation calculation is carried out under being transformed into navigational coordinate system by computing machine again, this process has been equivalent to a platform virtual in a computer, to be used as the reference of navigation calculation.Stable platform in the effect of this virtual platform and 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 very important role for SINS navigation procedure.Because the model of SINS initial alignment itself has nonlinear feature, therefore, rational non-linear filtering method to be adopted according to concrete requirement.

Engineering use the earliest and the most widely non-linear filtering method surely belong to EKF (ExtendKalmanFilter, EKF).Its core concept carries out linearization to nonlinear model at point of fixity exactly.But when system non-linear stronger, this filtering method precision is lower, and easily disperses.Even when system is discontinuous, EKF filtering just cannot be applied.Based on the viewpoint of " carrying out approximate comparison nonlinear function of wanting to probability distribution to be similar to easily a lot ", Julier and Uhlmann proposes Unscented kalman filtering (UnscentedKalmanFilter, UKF).It approaches average through obeying gauss' condition distribution variable and variance by deterministic Sigma point.Volume Kalman filtering (CubatureKalmanFilter, CKF) is first put forward by IenkaranArasaratnam and SimonHaykin.Its core concept is Cubature conversion, namely by the Cubature point of choosing 2n weights such as grade, the variable of the Gaussian distributed after nonlinear transformation is carried out to the calculating of average and variance.CKF filtering is derived according to Spherical-RadicalCubature criterion, has better stability, higher precision compared with UKF.From filtering method, CKF is a kind of special circumstances of UKF.But adopting of CKF is counted less, calculated amount is little, and filtering accuracy is also better than UKF.The Cubature conversion be connected in turn by head and the tail is applied on Gaussian filter and just creates CKF filtering, proposed when CKF filtering all has non-linear for state equation and measurement equation at first.

Summary of the invention

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

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

Step one, error characteristics according to 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 filtering initial value:

Order with wherein, x 0for the initial value of state variable, P 0for the initial error covariance matrix of state variable;

Step 3, average according to the state variable of current time with state error covariance matrix P k|k-1calculate 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-1cKF filtering method traditionally calculates;

Wherein, for the k-1 moment is to the one-step prediction of k moment state variable average, P k|k-1for the k-1 moment is to the one-step prediction of k moment state error covariance matrix; and P k|k-1calculating all obtained through state equation nonlinear propagation rear weight is cumulative by the Cubature point in k-1 moment, the weights of each point are 1/2n, and n is the dimension of system state variables;

Step 4, under state equation is non-linear, that measurement equation is linear condition, carry out state variable and measure variable time upgrade;

χ 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 co-variance 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 perform from step 3, namely complete the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF.

Invention effect:

For the concrete error model of SINS large misalignment angle initial alignment, propose a kind of filtering method, i.e. marginalisation volume Kalman filtering (Rao-Black-wellisedCubatureKalmanFilter, RBCKF).When state equation and measurement equation all have non-linear, traditional CKF 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 CKF, namely Cubature conversion is adopted to state equation, linear renewal is taked to measurement equation.Owing to no matter carrying out Cubature conversion or linear transformation for linear model, the precision after conversion is identical.Therefore, RBCKF while not losing filtering accuracy, can decrease the time of SINS initial alignment.

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

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

Two, technology of the present invention compares the time that CKF can shorten SINS initial alignment under Large azimuth angle;

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

Accompanying drawing explanation

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

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

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

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

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

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

Fig. 7 is the course misalignment steady-state error curve in l-G simulation test; Wherein,---represent that CKF estimates,-----represents that 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 one, error characteristics according to 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 filtering initial value:

Order with wherein, x 0for the initial value of state variable, P 0for the initial error covariance matrix of state variable;

Step 3, average according to the state variable of current time with state error covariance matrix P k|k-1calculate 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-1cKF filtering method traditionally calculates;

Wherein, for the k-1 moment is to the one-step prediction of k moment state variable average, P k|k-1for the k-1 moment is to the one-step prediction of k moment state error covariance matrix; and P k|k-1calculating all obtained through state equation nonlinear propagation rear weight is cumulative by the Cubature point in k-1 moment, the weights of each point are 1/2n, and n is the dimension of system state variables;

Step 4, under state equation is non-linear, that measurement equation is linear condition, carry out state variable and measure variable time upgrade;

χ 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 co-variance 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 perform from step 3, namely complete the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF.

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

In step 5, due to for linear equation, no matter be Cubature conversion or linear transformation, the precision after conversion is identical.Therefore, the filtering accuracy of RBCKF and CKF is identical.But calculated amount, when calculating measurement average, variance and cross-correlation covariance, RBCKF does not but again adopt 2n Cubature point and carries out the computing such as nonlinear transformation and square root factorization, thus saves the working time of algorithm.Meanwhile, 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 formula, RBCKF has carried out nm altogether 2+ 2n 2m+mn multiplication and m 2+ nm 2+ 2n 2m+mn sub-addition.And by contrast, CKF not only will regenerate Cubature point, carry out the nonlinear transformation measured, also will carry out 2mn+4nm simultaneously 2+ 4n 2m multiplication and m 2+ 2nm 2+ 2n 2m+8nm+2n 2sub-addition.Can find out, along with the increase of m and n, the calculated amount of RBCKF will be far smaller than the calculated amount of CKF.

Visible, RBCKF filtering algorithm is applied in SINS Initial Alignment of Large Azimuth Misalignment On, while not losing filtering accuracy, can greatly shortens the time of aligning.

Present embodiment effect:

For the concrete error model of SINS large misalignment angle initial alignment, propose a kind of filtering method, i.e. marginalisation volume Kalman filtering (Rao-Black-wellisedCubatureKalmanFilter, RBCKF).When state equation and measurement equation all have non-linear, traditional CKF 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 CKF, namely Cubature conversion is adopted to state equation, linear renewal is taked to measurement equation.Owing to no matter carrying out Cubature conversion or linear transformation for linear model, the precision after conversion is identical.Therefore, RBCKF while not losing filtering accuracy, can decrease the time of SINS initial alignment.

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

One, present embodiment compares the precision that traditional CKF does not lose filtering;

Two, the technology of present embodiment compares the time that CKF can shorten SINS initial alignment under Large azimuth angle;

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

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

The velocity error of east orientation, north orientation is chosen in general quiet pedestal Initial Alignment of Large Azimuth Misalignment On and the misalignment φ in three orientation x, φ yand φ zfor state variable, namely therefore, the state equation of its quiet pedestal initial alignment is as follows:

Wherein, for local latitude; ω iefor rotational-angular velocity of the earth; with be the gyroscopic drift of three axis; with the acceleration zero being three axis is inclined; f x, f yand f zfor the ratio output valve of accelerometer projects in calculating Department of Geography; C ijit is the element that carrier is tied in the direction matrix of Department of Geography; C ' ijthat carrier is tied to the element calculated in Department of Geography's matrix;

Choose east orientation and north orientation velocity error for 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 identical with embodiment one.

Embodiment three: present embodiment and embodiment one or two unlike: calculate Cubature point set in step 3 and be specially:

Square root factorization is carried out to error co-variance matrix, then chooses Cubature point set, 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 identical with embodiment one or two.

Embodiment four: one of present embodiment and embodiment one to three unlike: carry out in step 4 state variable and measure variable time upgrade be specially: in strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On, state equation is nonlinear 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 the Cubature point set in step 3 is calculated after the nonlinear transformation of state equation the one-step prediction average of state variable , and the renewal of the time of measurement amount does not convert through Cubature but directly carries out the one-step prediction average of linear transformation calculated amount measurement , 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 identical with one of embodiment one to three.

Embodiment five: one of present embodiment and embodiment one to four upgrade error co-variance matrix unlike: step 5 and be specially:

When the measurement carrying out filtering upgrades, need the error co-variance matrix P of computing mode variable k|k-1, measure the error co-variance matrix of variable and cross-correlation covariance matrix then these variablees are applied on Gaussian filter, just define RBCKF filtering algorithm;

Step May Day: in RBCKF, the error co-variance matrix P of state variable k|k-1calculating identical with the computing method of traditional CKF, still need to be calculated by Cubature point set, and the impact of state-noise will be included in, specifically count

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 co-variance matrix of variable and cross-correlation covariance matrix calculating different from traditional CKF, directly upgraded by linear equation, need the fusion carrying out nonlinear equation and linear equation, concrete derivation is as follows:

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 under being re-applied to Gaussian filter, just create 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 while not losing filtering accuracy, can substantially reduce the convergence time of filtering, is specifically compared as follows:

Assuming that measurement equation is: Z k=H kx k+ V k(wherein, H kmatrix for m × n) from whole filtering algorithm, RBCKF and CKF just measures average in calculating measure variance and cross-correlation covariance matrix time there is 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 identical with one of embodiment one to four.

Verified by following l-G simulation test:

Before SINS initial alignment, choose the initial value x (0)=0 of state variable; Initial east orientation, north orientation and sky are set as respectively to misalignment: first group: φ en=0.5 0, φ u=10 0; Second group: φ en=0.5 0, φ u=15 0; 3rd group: φ en=0.5 0, φ u=20 0; Calculate, assuming that the constant value drift of gyro is 0.1 according to medium accuracy gyro 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, 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; Meanwhile, the noise error covariance matrix of measurement equation is designated as R=diag{ (0.1m/s) 2(0.1m/s) 2.Under different azimuth misalignment, the filtering algorithm of CKF and RBCKF is as shown in table 1 for working time:

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

The working time of two kinds of filtering algorithms under different Large azimuth angle is given in table 1.Because measurement equation is linear, when the time upgrades, RBCKF have employed the linear mode upgraded, and compare the mode that 2n point is adopted in traditional C KF filtering, the calculated amount of RBCKF is little.Therefore, under identical azimuthal misalignment angle, be always less than the working time of traditional CKF filtering the working time of RBCKF.That is RBCKF can improve the alignment speed of inertial alignment.

Fig. 4 ~ 7 give at φ xy=0.5 0, φ z=10 0the estimation misalignment graph of errors in lower rolling, pitching and course.

Because initial misalignment only has azimuthal misalignment angle to be wide-angle, and horizontal misalignment is low-angle.Therefore, Fig. 7 gives the steady-state error curve map of azimuthal misalignment angle error.

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

To sum up, RBCKF filtering algorithm is compared to traditional C KF filtering algorithm, and the precision of filtering is identical, but from Riming time of algorithm, the working time being always less than traditional C KF working time of RBCKF under identical Large azimuth angle.Therefore, RBCKF while not losing filtering accuracy, can contribute to the time shortening strapdown inertial navitation system (SINS) aligning.

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 the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method based on RBCKF realizes according to the following steps:
Step one, error characteristics according to 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 filtering initial value:
Order with wherein, x 0for the initial value of state variable, P 0for the initial error covariance matrix of state variable;
Step 3, average according to the state variable in k-1 moment with state error covariance matrix P k-1| k-1calculate the Cubature point set in k-1 moment: χ i , k - 1 = 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 the state variable in described k-1 moment with state error covariance matrix P k-1|k-1cKF filtering method traditionally calculates;
Wherein, for the average of the state variable in k moment, P k|k-1for k moment state error covariance matrix; and P k|k-1calculating all obtained through state equation nonlinear propagation rear weight is cumulative by the Cubature point set in k-1 moment, the weights of each point are 1/2n, and n is the dimension of system state variables;
Step 4, under state equation is non-linear, that measurement equation is linear condition, carry out state variable and measure variable time upgrade;
χ 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 co-variance 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 - 1
P z ^ k z ^ k = H k P k | k - 1 H k T + R k
Make k=k+1, continue to perform from step 3, wherein, for cross-correlation covariance matrix, for measuring the error co-variance matrix of variable, namely complete 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 setting up Initial Alignment of Large Azimuth Misalignment On in step one is specially:
The velocity error δ v of east orientation, north orientation is chosen in quiet pedestal Initial Alignment of Large Azimuth Misalignment On x, δ v yand the misalignment φ in three orientation x, φ yand φ zfor 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, for local latitude; ω iefor rotational-angular velocity of the earth; with be the gyroscopic drift of three axis; with the acceleration zero being three axis is inclined; f x, f yand f zfor the specific force output valve of accelerometer projects in calculating Department of Geography; C ijit is the element that carrier is tied in the direction matrix of Department of Geography; C ' ijthat carrier is tied to the element calculated in Department of Geography's matrix;
Choose east orientation and north orientation velocity error δ v x, δ v yfor 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 Cubature point set in step 3 is specially:
Square root factorization is carried out to error co-variance matrix, then chooses Cubature point set, namely χ i , k - 1 = 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 of carrying out state variable in step 4 and measuring variable upgrades to be specially: in strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On, state equation is nonlinear and measurement equation is linear, and concrete mathematic(al) representation is as follows:
X · = f ( X ) + G W Z = H X + V
Time for state variable upgrades, and the Cubature point set in step 3 is calculated after the nonlinear transformation of state equation the one-step prediction average of state variable and the time renewal of measurement amount does not convert through Cubature but directly carries out the one-step prediction average of linear transformation calculated amount measurement 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 error co-variance matrix and is specially:
When the measurement carrying out filtering upgrades, need the error co-variance matrix P of computing mode variable k|k-1, measure the error co-variance matrix of variable and cross-correlation covariance matrix then these variablees are applied on Gaussian filter, just define RBCKF filtering algorithm;
Step May Day: in RBCKF, the error co-variance matrix P of state variable k|k-1calculating identical with the computing method of traditional CKF, still need to be calculated by Cubature point set, and will include the impact of state-noise in, specific formula for calculation is 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 - 1 Formula;
Measure the error co-variance matrix of variable and cross-correlation covariance matrix calculating different from traditional CKF, directly upgraded by linear equation, need the fusion carrying out nonlinear equation and linear equation, concrete derivation is as follows:
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 under being re-applied to Gaussian filter, just create RBCKF, the general type of Gaussian filter is as follows:
x k | k = x ^ k | k - 1 + W k ( z k - z ^ k | k - 1 ) 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 while not losing filtering accuracy, can substantially reduce the convergence time of filtering, is specifically compared as follows:
Assuming that measurement equation is: Z k=H kx k+ V kwherein, H kfor the matrix of m × n, from whole filtering algorithm, RBCKF and CKF just measures average in calculating measure variance and cross-correlation covariance matrix time there is 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 CN103344260A (en) 2013-10-09
CN103344260B true 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)

Families Citing this family (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
CN103727941B (en) * 2014-01-06 2016-04-13 东南大学 Based on the volume Kalman nonlinear combination air navigation aid of carrier system speeds match
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
CN105004351A (en) * 2015-05-14 2015-10-28 东南大学 SINS large-azimuth misalignment angle initial alignment method based on self-adaptation UPF
CN105180968B (en) * 2015-09-02 2018-06-01 北京天航华创科技股份有限公司 A kind of IMU/ magnetometers installation misalignment filters scaling method online
CN105806363B (en) * 2015-11-16 2018-08-21 东南大学 The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
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
CN106840194B (en) * 2016-09-20 2019-09-27 南京喂啊游通信科技有限公司 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,et al..Particle Filters for State Estimation of Jump Markov Linear Systems.《IEEE TRANSACTIONS ON SIGNAL PROCESSING》.2001,第49卷(第3期), *
混合线性/非线性模型的准高斯Rao-Blackwellized粒子滤波法;庄泽森等;《航空学报》;20080331;第29卷(第2期);450-455 *

Also Published As

Publication number Publication date
CN103344260A (en) 2013-10-09

Similar Documents

Publication Publication Date Title
CN103245360B (en) Carrier-borne aircraft rotation type strapdown inertial navigation system Alignment Method under swaying base
Wu et al. Observability of strapdown INS alignment: A global perspective
Cho et al. Robust positioning technique in low-cost DR/GPS for land navigation
CN104075715B (en) A kind of underwater navigation localization method of Combining with terrain and environmental characteristic
Han et al. A novel method to integrate IMU and magnetometers in attitude and heading reference systems
JP4789216B2 (en) Improved GPS cumulative delta distance processing method for navigation applications
CN103630137B (en) A kind of for the attitude of navigational system and the bearing calibration of course angle
CN104165642B (en) Method for directly correcting and compensating course angle of navigation system
CN104567931A (en) Course-drifting-error elimination method of indoor inertial navigation positioning
CN101246022B (en) Optic fiber gyroscope strapdown inertial navigation system two-position initial alignment method based on filtering
CN103776446B (en) A kind of pedestrian's independent navigation computation based on double MEMS-IMU
Mulder et al. Non-linear aircraft flight path reconstruction review and new advances
CN100516775C (en) Method for determining initial status of strapdown inertial navigation system
CN103900559B (en) A kind of high-precision attitude resolving system based on Interference Estimation
CN103759742B (en) Serial inertial navigation nonlinear alignment method based on fuzzy adaptivecontroller technology
CN102809377B (en) Aircraft inertia/pneumatic model Combinated navigation method
CN105043385A (en) Self-adaption Kalman filtering method for autonomous navigation positioning of pedestrians
CN103076015B (en) A kind of SINS/CNS integrated navigation system based on optimum correction comprehensively and air navigation aid thereof
CN101846510A (en) High-precision satellite attitude determination method based on star sensor and gyroscope
CN104061899B (en) A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation
CN102435763B (en) Measuring method for attitude angular velocity of spacecraft based on star sensor
Li et al. Optimization-based INS in-motion alignment approach for underwater vehicles
CN101419080B (en) Mini quick-connecting inertia measurement system zero speed correcting method
EP2555017B1 (en) Vehicle navigation on the basis of satellite positioning data and vehicle sensor data
CN105737823B (en) A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF

Legal Events

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

Granted publication date: 20160427

Termination date: 20180718

CF01 Termination of patent right due to non-payment of annual fee