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 PDFInfo
 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
Links
 238000001914 filtration Methods 0.000 claims abstract description 69
 230000001131 transforming Effects 0.000 claims description 12
 238000000034 methods Methods 0.000 claims description 8
 238000004364 calculation methods Methods 0.000 claims description 5
 230000001133 acceleration Effects 0.000 claims description 3
 230000001186 cumulative Effects 0.000 claims description 3
 238000009795 derivation Methods 0.000 claims description 2
 238000004088 simulation Methods 0.000 description 5
 230000000694 effects Effects 0.000 description 3
Abstract
Description
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 nonlinear filtering method to be adopted according to concrete requirement.
Engineering use the earliest and the most widely nonlinear 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 nonlinear 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 SphericalRadicalCubature 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 nonlinear for state equation and measurement equation at first.
Summary of the invention
The present invention is when wanting resolution system nonlinear 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 _{0}for the initial value of state variable, P _{0}for 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
_{kk1}calculate Cubature point set:
Wherein, for the k1 moment is to the onestep prediction of k moment state variable average, P _{kk1}for the k1 moment is to the onestep prediction of k moment state error covariance matrix; and P _{kk1}calculating all obtained through state equation nonlinear propagation rear weight is cumulative by the Cubature point in k1 moment, the weights of each point are 1/2n, and n is the dimension of system state variables;
Step 4, under state equation is nonlinear, that measurement equation is linear condition, carry out state variable and measure variable time upgrade;
Step 5, renewal error covariance matrix
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 (RaoBlackwellisedCubatureKalmanFilter, RBCKF).When state equation and measurement equation all have nonlinear, traditional CKF is a kind of very outstanding nonlinear filtering method.But the state equation of SINS large misalignment angle initial alignment has nonlinear, 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 nonlinear 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 nonlinear 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 lG simulation test; Wherein,represent that CKF estimates,represents that RBCKF estimates;
Fig. 5 is the pitching misalignment graph of errors in lG simulation test; Wherein,represent that CKF estimates,represents that RBCKF estimates;
Fig. 6 is the course misalignment graph of errors in lG simulation test; Wherein,represent that CKF estimates,represents that RBCKF estimates;
Fig. 7 is the course misalignment steadystate error curve in lG 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 _{0}for the initial value of state variable, P _{0}for 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
_{kk1}calculate Cubature point set:
Wherein, for the k1 moment is to the onestep prediction of k moment state variable average, P _{kk1}for the k1 moment is to the onestep prediction of k moment state error covariance matrix; and P _{kk1}calculating all obtained through state equation nonlinear propagation rear weight is cumulative by the Cubature point in k1 moment, the weights of each point are 1/2n, and n is the dimension of system state variables;
Step 4, under state equation is nonlinear, that measurement equation is linear condition, carry out state variable and measure variable time upgrade;
Step 5, renewal error covariance matrix
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 statespace 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 crosscorrelation 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
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 (RaoBlackwellisedCubatureKalmanFilter, RBCKF).When state equation and measurement equation all have nonlinear, traditional CKF is a kind of very outstanding nonlinear filtering method.But the state equation of SINS large misalignment angle initial alignment has nonlinear, 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 nonlinear 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 nonlinear 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}, φ _{y}and φ _{z}for state variable, namely therefore, the state equation of its quiet pedestal initial alignment is as follows:
Wherein, for local latitude; ω _{ie}for rotationalangular velocity of the earth; with be the gyroscopic drift of three axis; with the acceleration zero being three axis is inclined; f _{x}, f _{y}and f _{z}for the ratio output valve of accelerometer projects in calculating Department of Geography; C _{ij}it is the element that carrier is tied in the direction matrix of Department of Geography; C ' _{ij}that 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 × 2}0 _{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 covariance matrix, then chooses Cubature point set, namely
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:
Time for state variable upgrades, and the Cubature point set in step 3 is calculated after the nonlinear transformation of state equation the onestep prediction average of state variable , and the renewal of the time of measurement amount does not convert through Cubature but directly carries out the onestep prediction average of linear transformation calculated amount measurement , detailed process is as shown in the formula calculating:
Embodiment five: one of present embodiment and embodiment one to four upgrade error covariance matrix unlike: step 5 and be specially:
When the measurement carrying out filtering upgrades, need the error covariance matrix P of computing mode variable _{kk1}, measure the error covariance matrix of variable and crosscorrelation covariance matrix then these variablees are applied on Gaussian filter, just define RBCKF filtering algorithm;
Step May Day: in RBCKF, the error covariance matrix P of state variable _{kk1}calculating identical with the computing method of traditional CKF, still need to be calculated by Cubature point set, and the impact of statenoise will be included in, specifically count
Calculate formula as
Measure the error covariance matrix of variable and crosscorrelation 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:
Step 5 two: will and under being reapplied to Gaussian filter, just create RBCKF, the general type of Gaussian filter is as follows:
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 _{k}x _{k}+ V _{k}(wherein, H _{k}matrix for m × n) from whole filtering algorithm, RBCKF and CKF just measures average in calculating measure variance and crosscorrelation covariance matrix time there is difference, in traditional C KF, this computing formula of three is as follows:
Other step and parameter identical with one of embodiment one to four.
Verified by following lG 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: φ _{e}=φ _{n}=0.5 ^{0}, φ _{u}=10 ^{0}; Second group: φ _{e}=φ _{n}=0.5 ^{0}, φ _{u}=15 ^{0}; 3rd group: φ _{e}=φ _{n}=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 ^{4}m/s ^{2}, random deviation is 0.5 × 10 ^{4}g;
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 φ _{x}=φ _{y}=0.5 ^{0}, φ _{z}=10 ^{0}the estimation misalignment graph of errors in lower rolling, pitching and course.
Because initial misalignment only has azimuthal misalignment angle to be wideangle, and horizontal misalignment is lowangle.Therefore, Fig. 7 gives the steadystate error curve map of azimuthal misalignment angle error.
As can be seen from Figure 7 CKF with RBCKF is identical for course misalignment steadystate error value, is3.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)
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201310302760.3A CN103344260B (en)  20130718  20130718  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)  20130718  20130718  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)  20131009 
CN103344260B true CN103344260B (en)  20160427 
Family
ID=49279076
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201310302760.3A CN103344260B (en)  20130718  20130718  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)
Publication number  Priority date  Publication date  Assignee  Title 

CN103575298A (en) *  20131114  20140212  哈尔滨工程大学  Selfregulationbased unscented Kalman filter (UKF) misalignment angle initialalignment method 
CN103727941B (en) *  20140106  20160413  东南大学  Based on the volume Kalman nonlinear combination air navigation aid of carrier system speeds match 
CN103727940B (en) *  20140115  20160504  东南大学  Nonlinear initial alignment method based on acceleration of gravity vector matching 
CN103791918A (en) *  20140210  20140514  哈尔滨工程大学  Polar region moving base alignment method for naval vessel strapdown inertial navigation system 
CN104374401A (en) *  20141015  20150225  哈尔滨工程大学  Compensating method of gravity disturbance in strapdown inertial navigation initial alignment 
CN104374405A (en) *  20141106  20150225  哈尔滨工程大学  MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering 
CN104567871B (en) *  20150112  20180724  哈尔滨工程大学  A kind of quaternary number Kalman filtering Attitude estimation method based on earth magnetism gradient tensor 
CN105004351A (en) *  20150514  20151028  东南大学  SINS largeazimuth misalignment angle initial alignment method based on selfadaptation UPF 
CN105180968B (en) *  20150902  20180601  北京天航华创科技股份有限公司  A kind of IMU/ magnetometers installation misalignment filters scaling method online 
CN105806363B (en) *  20151116  20180821  东南大学  The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF 
CN106123921B (en) *  20160710  20190524  北京工业大学  The unknown Alignment Method of the latitude of Strapdown Inertial Navigation System under the conditions of dynamic disturbance 
CN106840194B (en) *  20160920  20190927  南京喂啊游通信科技有限公司  A kind of Large azimuth angle linear alignment method 
Citations (1)
Publication number  Priority date  Publication date  Assignee  Title 

CN101915579A (en) *  20100715  20101215  哈尔滨工程大学  Novel CKF(Crankshaft Fluctuation Sensor)based SINS (Ship Inertial Navigation System) large misalignment angle initiallyaligning method 

2013
 20130718 CN CN201310302760.3A patent/CN103344260B/en not_active IP Right Cessation
Patent Citations (1)
Publication number  Priority date  Publication date  Assignee  Title 

CN101915579A (en) *  20100715  20101215  哈尔滨工程大学  Novel CKF(Crankshaft Fluctuation Sensor)based SINS (Ship Inertial Navigation System) large misalignment angle initiallyaligning method 
NonPatent Citations (2)
Title 

Arnaud Doucet,et al..Particle Filters for State Estimation of Jump Markov Linear Systems.《IEEE TRANSACTIONS ON SIGNAL PROCESSING》.2001,第49卷(第3期), * 
混合线性/非线性模型的准高斯RaoBlackwellized粒子滤波法;庄泽森等;《航空学报》;20080331;第29卷(第2期);450455 * 
Also Published As
Publication number  Publication date 

CN103344260A (en)  20131009 
Similar Documents
Publication  Publication Date  Title 

CN103245360B (en)  Carrierborne 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 lowcost 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)  Coursedriftingerror elimination method of indoor inertial navigation positioning  
CN101246022B (en)  Optic fiber gyroscope strapdown inertial navigation system twoposition initial alignment method based on filtering  
CN103776446B (en)  A kind of pedestrian's independent navigation computation based on double MEMSIMU  
Mulder et al.  Nonlinear 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 highprecision 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)  Selfadaption 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)  Highprecision 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.  Optimizationbased INS inmotion alignment approach for underwater vehicles  
CN101419080B (en)  Mini quickconnecting 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 nonpayment of annual fee 
Granted publication date: 20160427 Termination date: 20180718 

CF01  Termination of patent right due to nonpayment of annual fee 