# Error Prediction for SINS/GPS after GPS Outage Based on Hybrid KF-UKF

Authors: Baiqiang Zhang; Hairong Chu; Tingting Sun; Hongguang Jia; Lihong Guo; Yue Zhang
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

The performance of MEMS-SINS/GPS integrated system degrades evidently during GPS outage due to the poor error characteristics of low-cost IMU sensors. The normal EKF is unable to estimate SINS error accurately after GPS outage owing to the large nonlinear error caused by MEMS-IMU. Aiming to solve this problem, a hybrid KF-UKF algorithm for real-time SINS/GPS integration is presented in this paper. The linear and nonlinear SINS error models are discussed, respectively. When GPS works well, we fuse SINS and GPS with KF with linear SINS error model using normal EKF. In the case of GPS outage, we implement Unscented Transform to predict SINS error covariance with nonlinear SINS error model until GPS signal recovers. In the simulation test that we designed, an evident accuracy improvement in attitude and velocity could be noticed compared to the normal EKF method after the GPS signal recovered.


---

## Body

## 1. Introduction

Strapdown Inertial Navigation System (SINS) is a highly self-contained navigation system, utilizing Inertial Measurement Units (IMU) fixed to the vehicle to determine its attitude, velocity, and position by calculating the integral of the angular rates and accelerations that IMU measures. Global Positioning System (GPS) is a satellite based radio navigation system that can provide accurate velocity and position information for a vehicle equipped with a GPS receiver [1]. SINS is commonly integrated with GPS using Kalman Filter (KF) to combine both advantages of these two techniques so that SINS/GPS has complete navigation information, high updating rate, good long-term accuracy, and high reliability.In recent years, with the development of Microelectronical Mechanical System (MEMS), IMU can be manufactured quite small with very low costs. So MEMS-SINS/GPS integrated navigation systems have been widely used in many areas, such as land-vehicle navigation, Unmanned Aerial Vehicle (UAV) control, and tactical missile guidance [2]. Unfortunately, these low-cost MEMS inertial sensors have relatively poor error characteristics. Although we can compensate the deterministic part by calibration experiments, the random error, including noise, bias-drifts, and random-walk, will still cause further degradation of SINS performance [3]. In a practical application, GPS signal may encounter disturbance or obstacle and KF fails to estimate SINS errors when no GPS information is available, which causes two problems.The first problem is that the performance of SINS degrades very fast since MEMS-IMU has low accuracy and no measurement information can be used for KF to correct the errors. And when GPS signal recovers, another problem occurs. KF or the extended KF (EKF) only works when the system is linear or slightly nonlinear so that it can be approximated by linearization. However, the SINS error model may have changed and became strongly nonlinear during GPS outages since SINS errors have been growing very fast. Even after GPS signal recovery, the KF cannot estimate the system error correctly.The first problem has been studied for many years and several methods have been proposed to solve it. In [4], under the assumption that the land-vehicle has no slip on the ground, SINS errors are constrained by considering the movement path of the vehicle. In [5], an odometer is used to offer extra observation information for the KF in GPS outages. These two methods are easy to achieve but only work when the movement path of the vehicle is simple. So the majority of researches have been focusing on the online study methods using Artificial Neural Network (ANN) or Support Vector Machine (SVM) [6]. Nowadays, several advanced information fusion algorithms have been proposed, such as strong-tracking Kalman Filter (STKF) combined with wavelet neural network (WNN) [7], genetic algorithm based adaptive neurofuzzy inference system (GANFIS) [8], and Dempster-Shafer augmented Support Vector Machine (DS-SVM) [9]. By online training when GPS is working, these algorithms can be used to estimate the system errors and correct them during GPS outages. Although proved to be efficient in theory, these methods are seldom applied practically for their huge amounts of calculation. The second problem we mentioned above, however, has not drawn much attention all these years. In fact, most MEMS-SINS/GPS integrated navigation systems today still work in SINS alone mode if GPS signal breaks down. And there will be a significant degradation in the system performance when GPS signal recovers owing to the drawback of KF.In order to overcome the limitation of KF, Julier and Uhlmann proposed the Unscented Kalman Filter (UKF) in 1995 [10]. The basic approach to predict the state of a strongly nonlinear system in UKF is the Unscented Transform (UT). Based on UT, UKF is able to estimate a strongly nonlinear system and a three-order accuracy can be attainable [11]. According to the work of [12, 13] on low-cost INS initial alignment, the errors converge more quickly in UKF compared to EKF when the initial attitude errors and uncertainties are large. So the problem that the SINS error model becomes strongly nonlinear could be solved by implementing UKF. But UKF has a larger amount of calculation compared with KF or EKF because a great number of sample points must be calculated in UT [14]. For a MEMS-SINS/GPS integrated navigation system, the system model is slightly nonlinear when GPS is functional and it is not necessary to predict the state with UT. Then we come up with an idea of a hybrid UKF-EKF SINS/GPS fusion method. In this algorithm, UKF and EKF can be switched so that the filter is able to estimate the error of system in a nonlinear case but has a low calculation amount when the system is linear or slightly nonlinear.In this research, we aimed at improving the MEMS-SINS/GPS integrated navigation system performance after GPS outage. A hybrid KF-UKF algorithm for real-time MEMS-SINS/GPS integration is presented in the paper. First, we will discuss two kinds of SINS error models which are the nonlinear model and the approximately linear model. Then we will present the hybrid KF-UKF algorithm and its calculation progress. When GPS works well, we fuse SINS and GPS with KF as usual. In the case of GPS outage, we implement UT to predict SINS error covariance until GPS signal recovers. Finally, a simulation and an in-car experiment were designed to test the algorithm and the results are compared with common KF method.


## 2. SINS Error Model

### 2.1. The Definition of SINS Error

Usually, the integrated navigation system filter is designed as indirect filtering, which means the system state is selected as the error of the system parameters instead of the parameters themselves so that the model is less complex. The navigation system model is a function of SINS error and the errors of SINS are selected as the system state. They are a vector of the errors of position, attitude, velocity, gyroscopes, and accelerometers:(1)x=δrδvψδfibbδωibbT.The attitude error is defined as the Euler angleψ=ψx,ψy,ψzT between the real navigation platform n (n-frame, local north, east, and down) and the computed navigation platform n′. The n′ frame is achieved by rotating n-frame with respect to axes z, y, and x by the angles ψz, ψy, and ψx, respectively. Then we have three coordinate transformation matrixes:(2)CZ=cos⁡ψzsin⁡ψz0-sin⁡ψzcos⁡ψz0001,CY=cos⁡ψy0-sin⁡ψy010sin⁡ψy0cos⁡ψy,CX=1000cos⁡ψxsin⁡ψx0-sin⁡ψxcos⁡ψx.Denoted by attitude transition matrix, the attitude error is expressed as(3)Cbn′=Cnn′Cbn,Cnn′=CZCYCX=cos⁡ψycos⁡ψzcos⁡ψysin⁡ψz-sin⁡ψy-cos⁡ψxsin⁡ψz+sin⁡ψxsin⁡ψycos⁡ψzcos⁡ψxcos⁡ψz+sin⁡ψxsin⁡ψysin⁡ψzsin⁡ψxcos⁡ψysin⁡ψxsin⁡ψz+cos⁡ψxsin⁡ψycos⁡ψz-sin⁡ψxcos⁡ψz+cos⁡ψxsin⁡ψysin⁡ψzcos⁡ψxcos⁡ψy,where Cbn′ is the direction cosine matrix (DCM) from the body-frame (b-frame) to the computed navigation frame (n′-frame); Cbn is the DCM from b-frame to the real navigation frame (n-frame); Cnn′ is the DCM from n-frame to n′-frame [15].


### 2.2. Linear SINS Error Model

KF is only able to estimate the state when the system is linear. Fortunately, when SINS integrated with GPS, the SINS errors can be corrected every filtering period. And SINS errors accumulated in this period are pretty small. So we can neglect the higher order terms of the SINS error function and the model is approximately linear.The attitude error is approximated as(4)Cnn′≈I-ψ×.The attitude, the velocity, and the position error equations, respectively, are(5)ψ˙≈-ωinn×ψ+δωinn-Cbnδωibb+Cbnwgb,δv˙n≈fibn×ψ-2ωien+ωenn×δvn+δgn+Cbnδfibb+Cbnwab,δr˙n≈-ωenn×δrn+δvn,where ωinn is the angular rotation velocity of the navigation coordinate system with respect to the inertial frame; ωien is the earth rotation velocity; ωenn is the rotation vector from the e-frame to the n-frame; fibn is the specific force vector in n-frame; δgn is the error of the gravity vector in n-frame; wgb and wab are the noise of the gyroscopes and accelerators, respectively [16].And the SINS error model can be written as(6)x˙=Ftx+Gtw.So it is able to predict the system state by using a transform matrix. And KF is available when GPS works well.


### 2.3. Nonlinear SINS Error Model

Although the linear SINS model has been proved to be efficient in SINS/GPS integration, it may be not accurate enough if the SINS errors accumulate with time when the navigation system works in SINS alone mode. In this situation, we cannot neglect the nonlinear parts of the SINS error function and it is necessary to develop the nonlinear SINS error model.Now let us review (3). If we define ωnn′n′ as the angular rotation vector from the n-frame to the n′-frame and ψ˙=ψ˙xψ˙yψ˙zT as the Euler angle rates, then we have their relationship with the Euler angle:(7)ωnn′n′=ψ˙x00+CX0ψ˙y0+CXCY00ψ˙z.Without linearization, the attitude error differential equation can be derived from (7) as(8)ψ˙=Cψ-1ωnn′n′=10-sin⁡ψy0cos⁡ψxsin⁡ψxcos⁡ψy0-sin⁡ψxcos⁡ψxcos⁡ψy-1ωnn′n′,where Cψ is the transition matrix between n-frame and n′-frame [17].Based on (8), the attitude, the velocity, and the position error equations are expressed as(9)ψ˙=Cψ-1I-Cnn′ω~inn+Cnn′δωinn-Cbn′δωibb+Cψ-1Cbn′wgb,(10)δv˙n=I-Cn′nCbn′f~ibb-2ω~ien+ω~enn×δvn-2δωien+δωenn×v~n+2δωien+δωenn×δvn+δgn+Cn′nCbn′δfibb+Cn′nCbn′wab,(11)δL˙=v~NR~N+h~-v~N-δvNR~N-δRN+h~-δh,δλ˙=v~EsecL~R~E+h~-v~E-δvEsec⁡L~-δLR~E-δRE+h~-δh,δh˙=-δvD,where ω~inn is the computed angular rotation velocity of the navigation coordinate system with respect to the inertial frame; ω~ien is the computed earth rotation velocity; ω~enn is the computed rotation vector from the e-frame to the n-frame; their errors are δωinn, δωien, and δωenn; f~ibb is the specific force that the accelerators measure; v~n, L~, λ~, and h~ and δL, δλ, δh, and δvn are the computed velocity, latitude, longitude, and height and their errors, respectively; R~N, R~E and δRN, δRE are the radius of the meridian and the prime vertical circle and their errors, respectively [18].And the SINS error model is written as(12)x˙=fx,t+gx,tw.Now we cannot predict the SINS errors by transform matrix. As we presented in the next part, the UT method is used to predict the system state.


## 2.1. The Definition of SINS Error

Usually, the integrated navigation system filter is designed as indirect filtering, which means the system state is selected as the error of the system parameters instead of the parameters themselves so that the model is less complex. The navigation system model is a function of SINS error and the errors of SINS are selected as the system state. They are a vector of the errors of position, attitude, velocity, gyroscopes, and accelerometers:(1)x=δrδvψδfibbδωibbT.The attitude error is defined as the Euler angleψ=ψx,ψy,ψzT between the real navigation platform n (n-frame, local north, east, and down) and the computed navigation platform n′. The n′ frame is achieved by rotating n-frame with respect to axes z, y, and x by the angles ψz, ψy, and ψx, respectively. Then we have three coordinate transformation matrixes:(2)CZ=cos⁡ψzsin⁡ψz0-sin⁡ψzcos⁡ψz0001,CY=cos⁡ψy0-sin⁡ψy010sin⁡ψy0cos⁡ψy,CX=1000cos⁡ψxsin⁡ψx0-sin⁡ψxcos⁡ψx.Denoted by attitude transition matrix, the attitude error is expressed as(3)Cbn′=Cnn′Cbn,Cnn′=CZCYCX=cos⁡ψycos⁡ψzcos⁡ψysin⁡ψz-sin⁡ψy-cos⁡ψxsin⁡ψz+sin⁡ψxsin⁡ψycos⁡ψzcos⁡ψxcos⁡ψz+sin⁡ψxsin⁡ψysin⁡ψzsin⁡ψxcos⁡ψysin⁡ψxsin⁡ψz+cos⁡ψxsin⁡ψycos⁡ψz-sin⁡ψxcos⁡ψz+cos⁡ψxsin⁡ψysin⁡ψzcos⁡ψxcos⁡ψy,where Cbn′ is the direction cosine matrix (DCM) from the body-frame (b-frame) to the computed navigation frame (n′-frame); Cbn is the DCM from b-frame to the real navigation frame (n-frame); Cnn′ is the DCM from n-frame to n′-frame [15].


## 2.2. Linear SINS Error Model

KF is only able to estimate the state when the system is linear. Fortunately, when SINS integrated with GPS, the SINS errors can be corrected every filtering period. And SINS errors accumulated in this period are pretty small. So we can neglect the higher order terms of the SINS error function and the model is approximately linear.The attitude error is approximated as(4)Cnn′≈I-ψ×.The attitude, the velocity, and the position error equations, respectively, are(5)ψ˙≈-ωinn×ψ+δωinn-Cbnδωibb+Cbnwgb,δv˙n≈fibn×ψ-2ωien+ωenn×δvn+δgn+Cbnδfibb+Cbnwab,δr˙n≈-ωenn×δrn+δvn,where ωinn is the angular rotation velocity of the navigation coordinate system with respect to the inertial frame; ωien is the earth rotation velocity; ωenn is the rotation vector from the e-frame to the n-frame; fibn is the specific force vector in n-frame; δgn is the error of the gravity vector in n-frame; wgb and wab are the noise of the gyroscopes and accelerators, respectively [16].And the SINS error model can be written as(6)x˙=Ftx+Gtw.So it is able to predict the system state by using a transform matrix. And KF is available when GPS works well.


## 2.3. Nonlinear SINS Error Model

Although the linear SINS model has been proved to be efficient in SINS/GPS integration, it may be not accurate enough if the SINS errors accumulate with time when the navigation system works in SINS alone mode. In this situation, we cannot neglect the nonlinear parts of the SINS error function and it is necessary to develop the nonlinear SINS error model.Now let us review (3). If we define ωnn′n′ as the angular rotation vector from the n-frame to the n′-frame and ψ˙=ψ˙xψ˙yψ˙zT as the Euler angle rates, then we have their relationship with the Euler angle:(7)ωnn′n′=ψ˙x00+CX0ψ˙y0+CXCY00ψ˙z.Without linearization, the attitude error differential equation can be derived from (7) as(8)ψ˙=Cψ-1ωnn′n′=10-sin⁡ψy0cos⁡ψxsin⁡ψxcos⁡ψy0-sin⁡ψxcos⁡ψxcos⁡ψy-1ωnn′n′,where Cψ is the transition matrix between n-frame and n′-frame [17].Based on (8), the attitude, the velocity, and the position error equations are expressed as(9)ψ˙=Cψ-1I-Cnn′ω~inn+Cnn′δωinn-Cbn′δωibb+Cψ-1Cbn′wgb,(10)δv˙n=I-Cn′nCbn′f~ibb-2ω~ien+ω~enn×δvn-2δωien+δωenn×v~n+2δωien+δωenn×δvn+δgn+Cn′nCbn′δfibb+Cn′nCbn′wab,(11)δL˙=v~NR~N+h~-v~N-δvNR~N-δRN+h~-δh,δλ˙=v~EsecL~R~E+h~-v~E-δvEsec⁡L~-δLR~E-δRE+h~-δh,δh˙=-δvD,where ω~inn is the computed angular rotation velocity of the navigation coordinate system with respect to the inertial frame; ω~ien is the computed earth rotation velocity; ω~enn is the computed rotation vector from the e-frame to the n-frame; their errors are δωinn, δωien, and δωenn; f~ibb is the specific force that the accelerators measure; v~n, L~, λ~, and h~ and δL, δλ, δh, and δvn are the computed velocity, latitude, longitude, and height and their errors, respectively; R~N, R~E and δRN, δRE are the radius of the meridian and the prime vertical circle and their errors, respectively [18].And the SINS error model is written as(12)x˙=fx,t+gx,tw.Now we cannot predict the SINS errors by transform matrix. As we presented in the next part, the UT method is used to predict the system state.


## 3. SINS/GPS Integration Algorithm

### 3.1. Kalman Filter

Developed in 1960s by Kalman, KF has been proved to be a powerful optimal estimation theory for linear systems. In SINS/GPS integration, KF is triggered in every GPS update epoch using the measurement information, that is, the difference of the velocity and position between SINS and GPS. Then the errors of SINS could be estimated and corrected. When GPS works well, no SINS error is accumulated and the navigation errors are bounded. Thus the linear SINS error model is accurate enough since the nonlinear parts could be ignored.The linear SINS error transition matrix is discretized as follows [19]:(13)Φk∣k-1≈I+Fk-1ΔT+12Fk-12ΔT2,Qk-1≈Gk-1qGk-1TΔT+12Fk-1Gk-1qGk-1T+Gk-1qGk-1TFk-1TΔT2,where F(t) is the coefficient matrix of the state equation; ΔT is the filter cycle; q is the covariance matrix of SINS sensors; and G(t) is the coefficient matrix of the state noise.For a loosely coupled SINS/GNSS system, the measurement equation is [20](14)z=Hx+v,where z is the difference of the velocity and position between SINS and GPS; H=I6×606×9; v is the noise standard deviation vector.KF algorithm is committed by using such formulas [21]:(15)x^k∣k-1=Φk∣k-1x^k-1,(16)Pk∣k-1=Φk∣k-1Pk-1Φk∣k-1T+Qk-1,(17)Kk=Pk∣k-1HkTHkPk∣k-1HkT+Rk-1,(18)x^k=x^k∣k-1+Kkzk-Hkx^k∣k-1,(19)Pk=I-KkHkPk∣k-1.In SINS/GPS integration, a close-loop configuration is used. In every KF epoch when the SINS errors are corrected, the statex^k is set to be zero.


### 3.2. Unscented Kalman Filter

As we mentioned above, KF is only available when the state equation is linear because it is not feasible to predict the state by transition matrix for a nonlinear system. If we neglect the higher order terms, KF will introduce errors which cannot be ignored for a strongly nonlinear system [22]. To predict the state of a nonlinear system, UT is used in UKF by generating a series of sample points to simulate the transfer of the state, which could achieve an accuracy of three orders.In UT, the state is predicted in three steps [23]. First, sigma points should be constructed:(20)χk-10=x^k-1χk-1i=x^k-1+n+λPk-1ci,i=1,2,…,nχk-1i=x^k-1-n+λPk-1ci-n,i=n+1,n+2,…,2n,where n is the dimension of the state vector x;  λ=α2(n+κ)-n, which decides the weights of the distribution of the sigma points and, generally, κ=3-n while 10-4≤α≤1. And n+λPk-1ci is the i column of the Cholesky decomposition of the matrix n+λPk-1.Second, the states are predicted with every sigma point. This step is executed by solving the error state differential equations with four-order Runge-Kutta method:(21)ξk∣k-1i=fχk-1ii=1,2…,2n.Finally, the states are weighted and summarized so we obtain the predicted statex^k∣k-1 and the error covariance matrix Pk∣k-1:(22)x^k∣k-1=∑i=02nWimξk∣k-1i,Pk∣k-1=∑i=02nWicξk∣k-1i-x^k∣k-1ξk∣k-1i-x^k∣k-1T+Qk-1,where Qk-1 is the covariance matrix of the state noise; Wim and Wic are the weights of the sigma points, which could be calculated as(23)W0m=λn+λ,W0c=λn+λ+1-α2+β,Wim=Wic=λ2n+λ,i=1,2…,2n,where α and λ have been given in the first step and β is assigned according to the distribution character of the state error. In this case, β=2.In loosely coupled SIN/GPS, the measurement equation is linear as we saw in (14). So after we predict the state x^k∣k-1 and the error covariance matrix Pk∣k-1, we could estimate the SINS errors with formulas (17) to (19).


### 3.3. Hybrid KF-UKF Algorithm

In this part we discussed the architecture of the hybrid KF-UKF algorithm. As shown in Figure1, SINS information is updated together with GPS. When GPS information is received, its availability is judged so that which method is to going be executed can be determined.Figure 1
The architecture of the hybrid KF-UKF algorithm.If GPS information is available, SINS and GPS can be integrated by normal KF method, as shown in Figure2. First, linear SINS error equation (6) is discretized by formula (13) with SINS information at time t=k-1. Then one-step state prediction and error covariance prediction are calculated with formulas (15) and (16) and the filter gain is calculated with formula (17). Afterwards, the state vector, which is the SINS error, and error covariance at time t=k are estimated with GPS information at t=k. Finally, the SINS error estimated at t=k is feedback to SINS, correcting the SINS error, and the same procedure is committed in the next cycle.Figure 2
Filter updates procedure when GPS is functioning well.During GPS signal blockage, the GPS information is unavailable and one-step state prediction is executed without measurement update. Now the nonlinear SINS error equation and UT are applied to predict the state and error covariance. As shown in Figure3, let us assume that the GPS signal breaks off at t=k and recovers at t=k+n. In order to estimate SINS error at t=k+n, it is necessary to predict the state x^k+n∣k-1 and error covariance Pk+n∣k-1. Suppose that k+s is a time instant in GPS outage, and x^k+s∣k-1,Pk+s∣k-1 are iterated by using x^k+s-1∣k-1,Pk+s-1∣k-1 and SINS information at time t=k+s-1, t=k+s-0.5, and t=k+s with formulas (24) to (27). This calculation is executed cycle by cycle until GPS signal recovers at t=k+n. When GPS receiver offered the velocity and position information of the vehicle at t=k+n, we already have the state x^k+n∣k-1 and error covariance Pk+n∣k-1. Then we can estimate SINS error at t=k+n with formulas (18) and (19), correct it, and go into the next filter cycle(24)χk+s-1∣k-10=x^k+s-1∣k-1,χk+s-1∣k-1i=x^k+s-1∣k-1+n+λPk+s-1∣k-1ci,i=1,2,…,n,χk+s-1∣k-1i=x^k+s-1∣k-1-n+λPk+s-1∣k-1ci-n,i=n+1,n+2,…,2n,(25)ξk+s∣k-1=fχk+s-1∣k-1,i=1,2…,2n,(26)x^k+s∣k-1=∑i=02nWimξk+s∣k-1i,(27)Pk+s∣k-1=∑i=02nWicξk+s∣k-1i-x^k+s∣k-1ξk+s∣k-1i-x^k+s∣k-1T+Qk-1.Figure 3
Filter updates procedure during GPS outage.


## 3.1. Kalman Filter

Developed in 1960s by Kalman, KF has been proved to be a powerful optimal estimation theory for linear systems. In SINS/GPS integration, KF is triggered in every GPS update epoch using the measurement information, that is, the difference of the velocity and position between SINS and GPS. Then the errors of SINS could be estimated and corrected. When GPS works well, no SINS error is accumulated and the navigation errors are bounded. Thus the linear SINS error model is accurate enough since the nonlinear parts could be ignored.The linear SINS error transition matrix is discretized as follows [19]:(13)Φk∣k-1≈I+Fk-1ΔT+12Fk-12ΔT2,Qk-1≈Gk-1qGk-1TΔT+12Fk-1Gk-1qGk-1T+Gk-1qGk-1TFk-1TΔT2,where F(t) is the coefficient matrix of the state equation; ΔT is the filter cycle; q is the covariance matrix of SINS sensors; and G(t) is the coefficient matrix of the state noise.For a loosely coupled SINS/GNSS system, the measurement equation is [20](14)z=Hx+v,where z is the difference of the velocity and position between SINS and GPS; H=I6×606×9; v is the noise standard deviation vector.KF algorithm is committed by using such formulas [21]:(15)x^k∣k-1=Φk∣k-1x^k-1,(16)Pk∣k-1=Φk∣k-1Pk-1Φk∣k-1T+Qk-1,(17)Kk=Pk∣k-1HkTHkPk∣k-1HkT+Rk-1,(18)x^k=x^k∣k-1+Kkzk-Hkx^k∣k-1,(19)Pk=I-KkHkPk∣k-1.In SINS/GPS integration, a close-loop configuration is used. In every KF epoch when the SINS errors are corrected, the statex^k is set to be zero.


## 3.2. Unscented Kalman Filter

As we mentioned above, KF is only available when the state equation is linear because it is not feasible to predict the state by transition matrix for a nonlinear system. If we neglect the higher order terms, KF will introduce errors which cannot be ignored for a strongly nonlinear system [22]. To predict the state of a nonlinear system, UT is used in UKF by generating a series of sample points to simulate the transfer of the state, which could achieve an accuracy of three orders.In UT, the state is predicted in three steps [23]. First, sigma points should be constructed:(20)χk-10=x^k-1χk-1i=x^k-1+n+λPk-1ci,i=1,2,…,nχk-1i=x^k-1-n+λPk-1ci-n,i=n+1,n+2,…,2n,where n is the dimension of the state vector x;  λ=α2(n+κ)-n, which decides the weights of the distribution of the sigma points and, generally, κ=3-n while 10-4≤α≤1. And n+λPk-1ci is the i column of the Cholesky decomposition of the matrix n+λPk-1.Second, the states are predicted with every sigma point. This step is executed by solving the error state differential equations with four-order Runge-Kutta method:(21)ξk∣k-1i=fχk-1ii=1,2…,2n.Finally, the states are weighted and summarized so we obtain the predicted statex^k∣k-1 and the error covariance matrix Pk∣k-1:(22)x^k∣k-1=∑i=02nWimξk∣k-1i,Pk∣k-1=∑i=02nWicξk∣k-1i-x^k∣k-1ξk∣k-1i-x^k∣k-1T+Qk-1,where Qk-1 is the covariance matrix of the state noise; Wim and Wic are the weights of the sigma points, which could be calculated as(23)W0m=λn+λ,W0c=λn+λ+1-α2+β,Wim=Wic=λ2n+λ,i=1,2…,2n,where α and λ have been given in the first step and β is assigned according to the distribution character of the state error. In this case, β=2.In loosely coupled SIN/GPS, the measurement equation is linear as we saw in (14). So after we predict the state x^k∣k-1 and the error covariance matrix Pk∣k-1, we could estimate the SINS errors with formulas (17) to (19).


## 3.3. Hybrid KF-UKF Algorithm

In this part we discussed the architecture of the hybrid KF-UKF algorithm. As shown in Figure1, SINS information is updated together with GPS. When GPS information is received, its availability is judged so that which method is to going be executed can be determined.Figure 1
The architecture of the hybrid KF-UKF algorithm.If GPS information is available, SINS and GPS can be integrated by normal KF method, as shown in Figure2. First, linear SINS error equation (6) is discretized by formula (13) with SINS information at time t=k-1. Then one-step state prediction and error covariance prediction are calculated with formulas (15) and (16) and the filter gain is calculated with formula (17). Afterwards, the state vector, which is the SINS error, and error covariance at time t=k are estimated with GPS information at t=k. Finally, the SINS error estimated at t=k is feedback to SINS, correcting the SINS error, and the same procedure is committed in the next cycle.Figure 2
Filter updates procedure when GPS is functioning well.During GPS signal blockage, the GPS information is unavailable and one-step state prediction is executed without measurement update. Now the nonlinear SINS error equation and UT are applied to predict the state and error covariance. As shown in Figure3, let us assume that the GPS signal breaks off at t=k and recovers at t=k+n. In order to estimate SINS error at t=k+n, it is necessary to predict the state x^k+n∣k-1 and error covariance Pk+n∣k-1. Suppose that k+s is a time instant in GPS outage, and x^k+s∣k-1,Pk+s∣k-1 are iterated by using x^k+s-1∣k-1,Pk+s-1∣k-1 and SINS information at time t=k+s-1, t=k+s-0.5, and t=k+s with formulas (24) to (27). This calculation is executed cycle by cycle until GPS signal recovers at t=k+n. When GPS receiver offered the velocity and position information of the vehicle at t=k+n, we already have the state x^k+n∣k-1 and error covariance Pk+n∣k-1. Then we can estimate SINS error at t=k+n with formulas (18) and (19), correct it, and go into the next filter cycle(24)χk+s-1∣k-10=x^k+s-1∣k-1,χk+s-1∣k-1i=x^k+s-1∣k-1+n+λPk+s-1∣k-1ci,i=1,2,…,n,χk+s-1∣k-1i=x^k+s-1∣k-1-n+λPk+s-1∣k-1ci-n,i=n+1,n+2,…,2n,(25)ξk+s∣k-1=fχk+s-1∣k-1,i=1,2…,2n,(26)x^k+s∣k-1=∑i=02nWimξk+s∣k-1i,(27)Pk+s∣k-1=∑i=02nWicξk+s∣k-1i-x^k+s∣k-1ξk+s∣k-1i-x^k+s∣k-1T+Qk-1.Figure 3
Filter updates procedure during GPS outage.


## 4. Simulation Test

### 4.1. Simulation Description and Parameters

For the sake of testing the hybrid KF-UKF algorithm, we design a simulation comparing this algorithm with normal EKF algorithm. The schematic diagram of the simulation is shown in Figure4. By using a path generator, we calculate the acceleration, angular rate, velocity, position, and attitude information of an aircraft. Stochastic errors were added to the acceleration and angular rate data to simulate the IMU sensor error. And the same process is done to the velocity and position information to simulate the GPS error. Besides, GPS outage flag is added to GPS data so the integration algorithm is able to judge whether GPS signal is lost. Finally, velocity, position, and attitude data calculated by hybrid KF-UKF and normal EKF algorithm are compared. The simulation parameters are shown in Table 1.Table 1
Main specifications of experiment instruments in the simulation test.
Equipment
Parameters
GPS receiver
Velocity accuracy
0.1 m/s (1σ)
Position accuracy
5 m (horizontal)10 m (vertical)
Output frequency
10 Hz
MEMS-IMU
Gyroscope
Bias
50°/h (1σ)
White noise
35°/h/√Hz
Accelerometer
Bias
2 mg (1σ)
White noise
0.3 mg/√Hz
Output frequency
200 HzFigure 4
Schematic diagram of the simulation.


### 4.2. Simulation Results and Analysis

In the simulation test, the aircraft has done a series of maneuvers which are listed in Table2 and its acceleration, angular rate, velocity, position, and attitude information was recorded. We calculate the navigation parameters of the aircraft by integrating SINS and GPS using the normal EKF and hybrid KF-UKF, respectively. Then the results were compared as is shown in Figure 5 and Table 3.Table 2
Aircraft maneuvers in the test.
Stage
Period
Aircraft maneuver
Parameters
1
0~30 s
Stationary
v = 0 m/s
2
30~60 s
Acceleration
v = 0 m/s to v = 100 m/s
3
60~75 s
Pitching
θ = 0° to θ = 30°
4
75~95 s
Straight flight
v = 100 m/s
5
95~120 s
Pitching
θ = 30° to θ = 0°
6
120~140 s
Straight flight
v = 100 m/s
7
140~165 s
Coordinate turn
ψ = 0° to ψ = 40°
8
165~190 s
Straight flight
v = 100 m/s
9
190~215 s
Coordinate turn
ψ = 40° to ψ = 0°
10
215~300 s
Straight flight
v = 100 m/sTable 3
Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage.
Outages length
Adopted approach
RMS attitude error (°)
RMS velocity error (m/s)
RMS position error (m)
Roll
Pitch
Yaw
North
East
Down
Latitude
Longitude
Height
50 s
EKF
0.0451
0.0566
0.2605
0.0256
0.0233
0.0331
1.1990
0.8245
3.3450
Hybrid KF-UKF
0.0438
0.0549
0.2499
0.0255
0.0233
0.0331
1.1984
0.8199
3.2310
100 s
EKF
0.0553
0.1050
0.2630
0.0286
0.0273
0.0367
1.1832
3.7463
3.0911
Hybrid KF-UKF
0.0508
0.0767
0.1706
0.0240
0.0259
0.0339
1.1701
3.7434
3.9175
150 s
EKF
0.0616
0.0590
0.1282
0.0363
0.0341
0.0479
1.3913
2.2053
3.1303
Hybrid KF-UKF
0.0489
0.0367
0.1140
0.0219
0.0237
0.0333
1.3876
2.1949
3.9078
200 s
EKF
0.0969
0.0560
1.7882
0.0431
0.0544
0.0619
1.5559
3.9014
3.6364
Hybrid KF-UKF
0.0586
0.0403
1.7131
0.0231
0.0318
0.0360
1.5552
3.8998
3.5416Figure 5
Comparison of attitude error between EKF and hybrid KF-UKF.Figure5 is the comparison of attitude error curves of EKF and hybrid KF-UKF, which are taken, for example. As we observe, the GPS signal was lost at t=70s and recovered at t=270s. During the 200 seconds GPS outage, the roll and pitch angle error grew with time because the accuracy of MEMS inertial sensors is very low and no GPS measurement information could be used for the filter to correct the SINS error. When the navigation system received GPS signal again at t=270s, the filter started to correct SINS error again. In the figure we can notice that, after GPS information was recovered, the attitude error reduced quickly. However, the attitude calculated by using hybrid KF-UKF is more accurate than that calculated by EKF, which is shown in the circle in the figure. This phenomenon appears for the reason that, as we described before, the MEMS-IMU sensor caused large nonlinear error during GPS outage. Based on the nonlinear SINS error model, UT is able to predict SINS errors better than EKF; thus the filter can correct the SINS errors quickly and accurately. And the similar phenomenon also occurs to the velocity, which is shown in Figure 6.Figure 6
Comparison of velocity error between EKF and hybrid KF-UKF.Table3 is the comparison of navigation error between EKF and hybrid KF-UKF after GPS outage. For attitude error, we can notice that the error of yaw is larger than that of roll and pitch owing to the poor observability of yawing angle in velocity/position integration mode; thus the filter is unable to estimate its error correctly. By comparing the RMS attitude error and velocity error after GPS outage, it is shown that the navigation error is lower if the hybrid KF-UKF algorithm is applied. Also it is shown in Table 3 that, with the GPS outage period increases, the hybrid KF-UKF algorithm was better and better compared to the EKF algorithm. That is because the error of an inertial navigation system accumulates with the increase. The longer the GPS outage is, the larger the nonlinear error the MEMS-IMU causes. And the hybrid KF-UKF algorithm will show more superiority to normal EKF algorithm. We can also notice that the latitude and longitude accuracy has little improvement on the contrary to attitude and velocity. And this can be explained by examining the SINS error equations (9) to (11). In formulas (9) and (10), the attitude error and velocity error are affected by the nonlinear terms Cnn′ and Cψ-1 whose values grew fast during GPS outage. However, when we refer to formula (11), we can see that the numerical value of δL, δRE, and δRN is very small and little nonlinear error is caused during GPS outage because the aircraft did not move quite far away during the simulation so the EKF is still able to correct its error accurately when GPS signal recovers.In Table4, we list the calculation amounts of EKF and UKF so that we can estimate the computational cost of the proposed combined algorithm. The calculation amounts of the algorithm are evaluated using the floating-point operations (flops), which is defined as the operation of adding, decreasing, multiplying, or dividing between two floating numbers. For example, if we add one (n×m) dimension matrix to another (n×m) dimension matrix, then nm flops have been generated in the computer. In Table 4, we suppose the dimension of the state vector is n and the measurement vector dimension is l. During the prediction updating period, 4n3+n2-n flops are generated in EKF while 6(2/3)n3+12n2+3n flops are generated in UKF. In SINS/GNSS integration, n=15 and l=6. So there will be 13710 flops in EKF and 25245 flops in UKF during the prediction updating period. What is more, in Unscented Transform the state prediction is performed by solving the error state differential equations with Runge-Kutta method (2n+1) times since each sigma point has to be predicted. That is to say, more than 25245 flops are generated in one filter recycle during GPS outages when we predict the SINS error using Unscented Transform. So in fact the computational cost of UKF is much larger than that in EKF in the prediction updating part; thus the hybrid UKF-EKF is recommended to reduce the computational burden for the computer. In hybrid UKF-EKF, the Unscented Transform, which is necessary to predict the nonlinear SINS error, only performs during GPS outages. Last but not least, the amount of flops in EKF during the measurement updating period is 2n3+6n2l+4nl2+l3+nl-n2, in this case, 17091, as well as that in UKF. This is for the reason that, in SINS/GNSS integration, the measurement equation is linear and no sigma point is needed in the measurement updating part. When GPS signal is functioning well, there will be 30801 flops in each filter cycle.Table 4
Comparison of computational cost of  EKF and UKF.
EKF
UKF (for linear measurement equation)
Step
Flops
Step
Flops
Prediction update
Generate sigma pointsχk-1
2
/
3
n
3
+
3
n
2
+
n
State predictionx^k|k-1
2
n
2
-
n
State predictionx^k|k-1
4
n
2
+
n
Error covariance predictionPk∣k-1
4
n
3
-
n
2
Error covariance predictionPk∣k-1
6
n
3
+
5
n
2
+
n
Subtotal
4
n
3
+
n
2
-
n
Subtotal
6
(
2
/
3
)
n
3
+
12
n
2
+
3
n
Measurement update
Filter gainKk
4
n
2
l
+
4
n
l
2
+
l
3
-
3
n
l
Filter gainKk
4
n
2
l
+
4
n
l
2
+
l
3
-
3
n
l
State updatex^k
4
n
l
State updatex^k
4
n
l
Error covariance updatePk
2
n
3
+
2
n
2
l
-
n
2
Error covariance updatePk
2
n
3
+
2
n
2
l
-
n
2
Subtotal
2
n
3
+
6
n
2
l
+
4
n
l
2
+
l
3
+
n
l
-
n
2
Subtotal
2
n
3
+
6
n
2
l
+
4
n
l
2
+
l
3
+
n
l
-
n
2
In total
6
n
3
+
6
n
2
l
+
4
n
l
2
+
l
3
+
n
l
-
n
In total
8
(
2
/
3
)
n
3
+
6
n
2
l
+
4
n
l
2
+
l
3
+
11
n
2
+
n
l
+
3
n
n
=
15, l=6
30801
n
=
15
,
l
=
6
42336


## 4.1. Simulation Description and Parameters

For the sake of testing the hybrid KF-UKF algorithm, we design a simulation comparing this algorithm with normal EKF algorithm. The schematic diagram of the simulation is shown in Figure4. By using a path generator, we calculate the acceleration, angular rate, velocity, position, and attitude information of an aircraft. Stochastic errors were added to the acceleration and angular rate data to simulate the IMU sensor error. And the same process is done to the velocity and position information to simulate the GPS error. Besides, GPS outage flag is added to GPS data so the integration algorithm is able to judge whether GPS signal is lost. Finally, velocity, position, and attitude data calculated by hybrid KF-UKF and normal EKF algorithm are compared. The simulation parameters are shown in Table 1.Table 1
Main specifications of experiment instruments in the simulation test.
Equipment
Parameters
GPS receiver
Velocity accuracy
0.1 m/s (1σ)
Position accuracy
5 m (horizontal)10 m (vertical)
Output frequency
10 Hz
MEMS-IMU
Gyroscope
Bias
50°/h (1σ)
White noise
35°/h/√Hz
Accelerometer
Bias
2 mg (1σ)
White noise
0.3 mg/√Hz
Output frequency
200 HzFigure 4
Schematic diagram of the simulation.


## 4.2. Simulation Results and Analysis

In the simulation test, the aircraft has done a series of maneuvers which are listed in Table2 and its acceleration, angular rate, velocity, position, and attitude information was recorded. We calculate the navigation parameters of the aircraft by integrating SINS and GPS using the normal EKF and hybrid KF-UKF, respectively. Then the results were compared as is shown in Figure 5 and Table 3.Table 2
Aircraft maneuvers in the test.
Stage
Period
Aircraft maneuver
Parameters
1
0~30 s
Stationary
v = 0 m/s
2
30~60 s
Acceleration
v = 0 m/s to v = 100 m/s
3
60~75 s
Pitching
θ = 0° to θ = 30°
4
75~95 s
Straight flight
v = 100 m/s
5
95~120 s
Pitching
θ = 30° to θ = 0°
6
120~140 s
Straight flight
v = 100 m/s
7
140~165 s
Coordinate turn
ψ = 0° to ψ = 40°
8
165~190 s
Straight flight
v = 100 m/s
9
190~215 s
Coordinate turn
ψ = 40° to ψ = 0°
10
215~300 s
Straight flight
v = 100 m/sTable 3
Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage.
Outages length
Adopted approach
RMS attitude error (°)
RMS velocity error (m/s)
RMS position error (m)
Roll
Pitch
Yaw
North
East
Down
Latitude
Longitude
Height
50 s
EKF
0.0451
0.0566
0.2605
0.0256
0.0233
0.0331
1.1990
0.8245
3.3450
Hybrid KF-UKF
0.0438
0.0549
0.2499
0.0255
0.0233
0.0331
1.1984
0.8199
3.2310
100 s
EKF
0.0553
0.1050
0.2630
0.0286
0.0273
0.0367
1.1832
3.7463
3.0911
Hybrid KF-UKF
0.0508
0.0767
0.1706
0.0240
0.0259
0.0339
1.1701
3.7434
3.9175
150 s
EKF
0.0616
0.0590
0.1282
0.0363
0.0341
0.0479
1.3913
2.2053
3.1303
Hybrid KF-UKF
0.0489
0.0367
0.1140
0.0219
0.0237
0.0333
1.3876
2.1949
3.9078
200 s
EKF
0.0969
0.0560
1.7882
0.0431
0.0544
0.0619
1.5559
3.9014
3.6364
Hybrid KF-UKF
0.0586
0.0403
1.7131
0.0231
0.0318
0.0360
1.5552
3.8998
3.5416Figure 5
Comparison of attitude error between EKF and hybrid KF-UKF.Figure5 is the comparison of attitude error curves of EKF and hybrid KF-UKF, which are taken, for example. As we observe, the GPS signal was lost at t=70s and recovered at t=270s. During the 200 seconds GPS outage, the roll and pitch angle error grew with time because the accuracy of MEMS inertial sensors is very low and no GPS measurement information could be used for the filter to correct the SINS error. When the navigation system received GPS signal again at t=270s, the filter started to correct SINS error again. In the figure we can notice that, after GPS information was recovered, the attitude error reduced quickly. However, the attitude calculated by using hybrid KF-UKF is more accurate than that calculated by EKF, which is shown in the circle in the figure. This phenomenon appears for the reason that, as we described before, the MEMS-IMU sensor caused large nonlinear error during GPS outage. Based on the nonlinear SINS error model, UT is able to predict SINS errors better than EKF; thus the filter can correct the SINS errors quickly and accurately. And the similar phenomenon also occurs to the velocity, which is shown in Figure 6.Figure 6
Comparison of velocity error between EKF and hybrid KF-UKF.Table3 is the comparison of navigation error between EKF and hybrid KF-UKF after GPS outage. For attitude error, we can notice that the error of yaw is larger than that of roll and pitch owing to the poor observability of yawing angle in velocity/position integration mode; thus the filter is unable to estimate its error correctly. By comparing the RMS attitude error and velocity error after GPS outage, it is shown that the navigation error is lower if the hybrid KF-UKF algorithm is applied. Also it is shown in Table 3 that, with the GPS outage period increases, the hybrid KF-UKF algorithm was better and better compared to the EKF algorithm. That is because the error of an inertial navigation system accumulates with the increase. The longer the GPS outage is, the larger the nonlinear error the MEMS-IMU causes. And the hybrid KF-UKF algorithm will show more superiority to normal EKF algorithm. We can also notice that the latitude and longitude accuracy has little improvement on the contrary to attitude and velocity. And this can be explained by examining the SINS error equations (9) to (11). In formulas (9) and (10), the attitude error and velocity error are affected by the nonlinear terms Cnn′ and Cψ-1 whose values grew fast during GPS outage. However, when we refer to formula (11), we can see that the numerical value of δL, δRE, and δRN is very small and little nonlinear error is caused during GPS outage because the aircraft did not move quite far away during the simulation so the EKF is still able to correct its error accurately when GPS signal recovers.In Table4, we list the calculation amounts of EKF and UKF so that we can estimate the computational cost of the proposed combined algorithm. The calculation amounts of the algorithm are evaluated using the floating-point operations (flops), which is defined as the operation of adding, decreasing, multiplying, or dividing between two floating numbers. For example, if we add one (n×m) dimension matrix to another (n×m) dimension matrix, then nm flops have been generated in the computer. In Table 4, we suppose the dimension of the state vector is n and the measurement vector dimension is l. During the prediction updating period, 4n3+n2-n flops are generated in EKF while 6(2/3)n3+12n2+3n flops are generated in UKF. In SINS/GNSS integration, n=15 and l=6. So there will be 13710 flops in EKF and 25245 flops in UKF during the prediction updating period. What is more, in Unscented Transform the state prediction is performed by solving the error state differential equations with Runge-Kutta method (2n+1) times since each sigma point has to be predicted. That is to say, more than 25245 flops are generated in one filter recycle during GPS outages when we predict the SINS error using Unscented Transform. So in fact the computational cost of UKF is much larger than that in EKF in the prediction updating part; thus the hybrid UKF-EKF is recommended to reduce the computational burden for the computer. In hybrid UKF-EKF, the Unscented Transform, which is necessary to predict the nonlinear SINS error, only performs during GPS outages. Last but not least, the amount of flops in EKF during the measurement updating period is 2n3+6n2l+4nl2+l3+nl-n2, in this case, 17091, as well as that in UKF. This is for the reason that, in SINS/GNSS integration, the measurement equation is linear and no sigma point is needed in the measurement updating part. When GPS signal is functioning well, there will be 30801 flops in each filter cycle.Table 4
Comparison of computational cost of  EKF and UKF.
EKF
UKF (for linear measurement equation)
Step
Flops
Step
Flops
Prediction update
Generate sigma pointsχk-1
2
/
3
n
3
+
3
n
2
+
n
State predictionx^k|k-1
2
n
2
-
n
State predictionx^k|k-1
4
n
2
+
n
Error covariance predictionPk∣k-1
4
n
3
-
n
2
Error covariance predictionPk∣k-1
6
n
3
+
5
n
2
+
n
Subtotal
4
n
3
+
n
2
-
n
Subtotal
6
(
2
/
3
)
n
3
+
12
n
2
+
3
n
Measurement update
Filter gainKk
4
n
2
l
+
4
n
l
2
+
l
3
-
3
n
l
Filter gainKk
4
n
2
l
+
4
n
l
2
+
l
3
-
3
n
l
State updatex^k
4
n
l
State updatex^k
4
n
l
Error covariance updatePk
2
n
3
+
2
n
2
l
-
n
2
Error covariance updatePk
2
n
3
+
2
n
2
l
-
n
2
Subtotal
2
n
3
+
6
n
2
l
+
4
n
l
2
+
l
3
+
n
l
-
n
2
Subtotal
2
n
3
+
6
n
2
l
+
4
n
l
2
+
l
3
+
n
l
-
n
2
In total
6
n
3
+
6
n
2
l
+
4
n
l
2
+
l
3
+
n
l
-
n
In total
8
(
2
/
3
)
n
3
+
6
n
2
l
+
4
n
l
2
+
l
3
+
11
n
2
+
n
l
+
3
n
n
=
15, l=6
30801
n
=
15
,
l
=
6
42336


## 5. Conclusion

This paper presents a hybrid KF-UKF algorithm for real-time MEMS-SINS/GPS integration. The linear and nonlinear SINS models are discussed. The flowchart of the hybrid KF-UKF algorithm is described. The SINS error is estimated and corrected with linear SINS model when GPS is functioning well while it is predicted with nonlinear SINS model by applying Unscented Transform during GPS outage. The simulation results indicate that, compared to normal EKF algorithm, the hybrid KF-UKF algorithm is able to predict the SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS signal was regained. The results also show that the hybrid KF-UKF algorithm is more efficient for attitude error prediction but the effect on position error is not evident. Compared with normal UKF, the hybrid KF-UKF algorithm has low calculation amount. In this paper, the remaining problem which we have not solved is that SINS errors still grow fast during GPS outage. So, in our future work, we may combine the UKF with ANN or SVM to create a new algorithm to reduce the SINS errors during GPS outage.



---

*Source: 239426-2015-09-28.xml*