Robust cubature Kalman filter method for the nonlinear alignment of SINS

Shi-luo Guo,Ying-jie Sun,Li-min Chang,Yang Li

Department of Instrument Electric,Aviation University Air Force,Changchun,People’s Republic of China

Keywords:SINS Nonlinear alignment Cubature Kalman filter Robust Multiple fading factors Hypothesis test

ABSTRACT Nonlinear initial alignment is a significant research topic for strapdown inertial navigation system(SINS).Cubature Kalman filter(CKF)is a popular tool for nonlinear initial alignment.Standard CKF assumes that the statics of the observation noise are pre-given before the filtering process.Therefore,any unpredicted outliers in observation noise will decrease the stability of the filter.In view of this problem,improved CKF method with robustness is proposed.Multiple fading factors are introduced to rescale the observation noise covariance.Then the update stage of the filter can be autonomously tuned,and if there are outliers exist in the observations,the update should be less weighted.Under the Gaussian assumption of KF,the Mahalanobis distance of the innovation vector is supposed to be Chi-square distributed.Therefore a judging index based on Chi-square test is designed to detect the noise outliers,determining whether the fading tune are required.The proposed method is applied in the nonlinear alignment of SINS,and vehicle experiment proves the effective of the proposed method.

1.Introduction

Strapdown inertial navigation system(SINS)is a very popular autonomous navigation equipment,playing an irreplaceable role in the military field.As a reckoning system,SINS is vulnerable to its initial error[1-3],hence its initialization,called initial alignment,is of vital importance for SINS.Kalman filter(KF)is a commonly used alignment tool.Sometimes under the condition of large misalignment angle,the alignment model of SINS is nonlinear,hence the nonlinear extensions of KF,such as Extended Kalman filter(EKF)[4,5],unscented Kalman filter(UKF)[6,7]and cubature Kalman filter(CKF)[8-11]are used in the nonlinear alignment.EKF is the most celebrated nonlinear approximations of KF.However,the linearization of nonlinear system in EKF leads to approximation errors and cumbersome calculations.Comparatively,UKF has more advantages than EKF since its linearization and derivative free,and compared with UKF,the theoretical deduction of CKF is more rigorous,and its stability are better especially in higher order systems[12].

It can be proven that only when the process and observation noises are Gaussian distribution,the KF is theoretically optimal,which means it is consistent and unbiased[13].Besides,KF assumes that the mathematical characteristics of the system noise and the observation noise are constant and pre-defined.However,the priori statistical information is deficient to predict the noise outliers,especially in SINS alignment,due to uncertain carrier motion state,unknown observation conditions and varying noise characteristics of the inertial measurement units(IMU)et al.Under these conditions,the observation outliers may cause instability or even divergence of filtering result.Hence the robustness of the KF has become a significant research topic.Such as H∞filters[14],federated filters[15],M-estimation based filters[16],strong tracking Kalman filter[17]et al.In Ref.[18],a novel sigma-points update method is proposed to enhance the robustness of CKF,providing a useful reference for this work.

Fading Kalman filters(FKF)is an effective suboptimal improved form of KF[19].This method focuses on the harmonious between the state prediction and the observation update.Because of the moderate computational load and the excellent performance,this method is widely studied.FKF was initially designed to solve the system modelling errors of KF.By using fading factor,the process noise covariance was rescaled,resulting in an increased filter gain,and the update of the current observation will be more weighted,and meanwhile the weight of historical information will be decreased.Through subsequent researches[20-22],the basic principle and application method of FKF was gradually optimized.To solve the problems of observation noise outliers,reference[23]extended this fading method to the robust improvement of KF.In which the observation noise covariance was tuned by fading factor,so the filter gain can be decreased and the weight of the observational update will be less weighted.However,the existing methods are mainly focused on the linear filtering applications,and shows insufficient in adaptability in terms of the introduction method of the fading factor.

This study focuses on the non-linear alignment of SINS under condition of uncertain observation noise.The main works can be summarized as follows:Fading method is introduced to enhance the robustness of CKF.Multiple fading factors are adopted to inflate the observation noise covariance.Compared with the single fading factor in Ref.[21],the tune ability of multiple factors are more efficient.A judging index is designed to monitor the filter state,and determine the introducing time of the fading method.In a stable KF,the Mahalanobis distance of the innovation should be Chi-square distributed.Therefore a hypothesis test based on the Chi-square distribution of the innovation is designed.The test results will reflect the state of the filter,i.e.the rejection of the null hypothesis implies that the Gaussian assumption is false.In other words,observations may be disturbed by outliers,and then the fading method should be performed to decrease the observational update weight.

This paper is organized as follows.Fundamental knowledge such as the filter model of SINS,is given in Section 2.Basic principles of CKF and improved robust CKF is proposed in Section 3.Vehicle experiments are conducted in Section 4.At the end,some conclusions are drawn and the future work are presented.

2.Filter model

Commonly used nonlinear filter model of SINS is[8].

In Eq.(1),n is the local geographic coordinate frame,and the computational n frame from the SINS is n′.b represents the body frame.Superscript means that the variable is the projection in corresponding frame.For example,ωieis the rotational speed of the Earth,andrepresents its projection in the n frame.gnis the gravity vector.andare the measurement of accelerometers and gyroscopes.ωnenis the rotational rate of the n frame with respect to the Earth.L,λ,h represent the latitude,longitude,altitude respectively.δvnis the difference between the SINS velocity and the actual velocity.I3is the three-dimensional identity matrix.is the transformation between the n frame and the n′frame,and it can be expressed as,in which

In Eq.(2),φx,φyare the pitch and roll misalignment angles and φzis the yaw misalignment angle.Acan be given by

3.Proposal of algorithm

3.1.Cubature Kalman filter(CKF)

The equations of system state and observation are supposed as

Standard CKF equation can be divided into state prediction and update stage.

(1)State Prediction

The estimation of the system state at k-1 epoch is supposed as,and its estimation covariance is supposed to bePk-1.

Step 1:Cholesky decomposition

Step 2:Calculation of cubature point

In Eq.(6),and D is the dimension of system states.is cubature point-group.［1］indicate the intersections of a unit sphere and its coordinate axis.For example,if m=2,then［1］represent

Step 3:Substitute cubature point into state equation

Step 4:Prediction of the state

Step 5:Prediction covariance

(2)Update stage

Step 6:Cholesky decomposition

Step 7:Calculation of cubature point

Step 8:Substitute cubature point into observation equation

Step 9:Observational Prediction

Step 10:Autocorrelation covariance

Step 11:Cross-correlation covariance

Step 12:Calculation of filter gain

Step 13:State estimation

Step 14:Estimation covariance

It should be noted that,above equations from Eq.(5)to Eq.(19)are the complete nonlinear steps of CKF.As in the nonlinear alignment issues of SINS,sometimes although the filtering state equation is nonlinear,the observation equation is still linear,at this moment the update stage of CKF can simplified as the linear update in KF,i.e.Eq.(11)-(19)can be replaced by

It will be discussed later that,in this paper the observation equation of the alignment filter is a linear equation,so the filter update can be performed by Eqs.(20)-(22).

3.2.Fading factor

Measurement-noise-inflating method based on fading algorithm is adopted to increase the robustness of CKF.Multiple fading factors are introduced to inflate the observation noise covarianceRk.

whereSrk=diag｛s1s2···sm｝,siis the single fading factor.diag｛·｝represent the diagonal matrix,and m is the dimension of the observations.The calculation method of the fading factor include the analytical method[22]and the iterative method[23].Considering the computational load of CKF,the analytically calculated scaling factor is preferred in this paper.

The innovation of the filter is defined as

Under the Gaussian assumption,the statistical distribution of the innovationekis

Therefore the distribution covariance ofekis

It has been proved in Ref.[20-22]thatCkcan be estimated in the filtering process

In order to solve theSrk,we have

Substituting Eq.(23)into Eq.(28)yields

DefineNrkas follows

It can be derived from Eq.(29)that

where ri，iis the i th diagonal element ofRk.It should be noted that,in order to inflate the observation covariance,sishould be larger than 1.

3.3.Robust CKF

In conventional fading filters,the fading factor will be used if it is larger than one,without considering whether the outliers is existing or not.However,excessive using of fading factors will impact the normal structure of the filter and increase the computational load.Therefore,a judging method is designed to detect whether the fading factors are required.

Fig.1.GPS/INS integrated speed reference.

Fig.2.GPS/INS integrated position reference.

Fig.3.Actual velocity and contaminated velocity.

Fig.4.Pitch misalignment errors.

Since the innovationekis Gaussian distribution,then the Mahalanobis distance of the innovation vector should be chi-square distributed,and its freedom degree is exactly the dimension of the innovation vector.

Then a detective criterion can be designed based on hypothesis test.The null hypothesis is

where theγkis the judging index.By offline assigning a small value αas the significance level,the corresponding quantileξcan be predetermined,which is

Ifαis taken as a fairly small value(such as 0.01),｛γk＞ξ｝would be a low probability event.Thereforeγkshould be smaller thanξ with a high probability i.e.1-α.In other words,ifγkis larger thanξ,it means small probability event occurred.According to the principle of hypothesis test,the null hypothesis is false.It implies that some outliers exist.Then the fading factors,which calculated from Eq.(31),should be introduced into the filter by Eq.(23).

Thus in every recursion step of CKF,we getγkfrom Eq.(32),and perform the hypothesis-testing through Eqs.(33)and(34).If the hypothesis H0is false,fading algorithm should be introduced.

4.Experimental study

Since the proposed algorithm is designed for the nonlinear alignment of SINS.A vehicle test was performed to evaluate the proposed alignment algorithm.The inertial measurement units of the experimental SINS consists of the following parts:

(1)Laser gyroscopes,the drift rate is 0.007。/h（1σ）.

(2)Quartz accelerometers,the bias is 5×10-5g（1σ）.

A single-antenna GPS receiver is installed on the top of the car.The gyros measurements,accelerometers measurements and the GPS velocity are recorded.The sampling rates of the INS and the GPS are 125 Hz and 1 Hz respectively.A 20 min segment of the whole recorded data is used to test the alignment performance of the proposed algorithm.The motions of the vehicle can be presented by the GPS/INS integrated system,which are shown in Figs.1 and 2.

Fig.5.Roll misalignment errors.

Fig.6.Yaw misalignment errors.

The filtering state is chosen as

Velocity errors［δvnE，δvnN，δvnU］are assigned as the observations,and the velocity reference is GPS velocity.Therefore the observation equation is

whereHk=［I3×303×9］.The observation is three-dimension,hence the judging indexγkhas aχ2（3）distribution.In this experiment,the significance levelαis chosen as 0.01,then the corresponding quantileξis 11.345.

In order to simulate the case of large misalignment angles,the initial attitude error is set as[10°,10°,30°].Other filter conditions are set as

The first 10 min segment of the record date are used to perform the alignment.Two kinds of observation error are introduced in the actual observations,i.e.,unpredictable White Gauss noise and the outliers,to simulate the case of observation outliers.

(1)Unpredictable Gauss noise:Gauss noise with variance of 1（m/s）2is added to the velocity reference(GPS output).

(2)Outliers case:At 100 s-250 s,and 400 s-550 s,bias errors with the value of 10 m/s are intentionally added into the GPS velocity every 10 s.

Take the east velocity as an example,actual GPS velocity and the contaminated velocity by artificial errors are presented in Fig.3.

Standard CKF and RCKF(robust CKF proposed in this paper)are compared,and alignment results are evaluated by the GPS/SINS integrated navigation outputs.

The attitude misalignment angles after 10 min alignment are shown in the following figures.

Figs.4-6 prove that,RCKF shows better robustness under the disturbance of observation noise.The final misalignment errors of RCKF are 0.002°,0.003°,and 0.1°,while the final misalignment errors of CKF are 0.4°,0.1°,and 2.7°.

5.Conclusions

For the nonlinear alignment of the SINS,an improved CKF method with robustness is proposed in this paper.Fading algorithm is introduced in the CKF,and a robust improvement is proposed.The proposed method is applied on the nonlinear alignment of SINS.Vehicle experiments show that the proposed method performs good filtering robustness even under the disturbance of anomalous observation noise.At present the proposed method is more like an empiricist exploration than a rigorous mathematical proof.Thus,the following theoretical research needs to be further strengthened and the fault tolerance of the algorithm needs to be improved.

Declaration of competing interest

The authors declared that they have no conflicts of interest to this work.We declare that we do not have any commercial or associative interest that represents a conflict of interest in connection with the work submitted.

Acknowledgment

This work is supported by National Natural Science Foundation of China under Grant No.41574069.The Major National Projects of China under Grant No.GFZX0301040303.