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

Theperformance ofMEMS-SINS/GPS integrated systemdegrades evidently duringGPS 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.


Introduction
Strapdown Inertial Navigation System (SINS) is a highly selfcontained 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 randomwalk, 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], 2 Mathematical Problems in Engineering 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.

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: The attitude error is defined as the Euler angle  = [  ,   ,   ]  between the real navigation platform  (frame, local north, east, and down) and the computed navigation platform   .The   frame is achieved by rotating -frame with respect to axes , , and  by the angles   ,   , and   , respectively.Then we have three coordinate transformation matrixes: Denoted by attitude transition matrix, the attitude error is expressed as where C    is the direction cosine matrix (DCM) from the body-frame (-frame) to the computed navigation frame ( frame); C   is the DCM from -frame to the real navigation frame (-frame); C    is the DCM from -frame to   -frame [15].

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 The attitude, the velocity, and the position error equations, respectively, are where    is the angular rotation velocity of the navigation coordinate system with respect to the inertial frame;    is the earth rotation velocity;    is the rotation vector from the frame to the -frame; f   is the specific force vector in -frame; g  is the error of the gravity vector in -frame; w   and w   are the noise of the gyroscopes and accelerators, respectively [16].
And the SINS error model can be written as So it is able to predict the system state by using a transform matrix.And KF is available when GPS works well.

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      as the angular rotation vector from the -frame to the   -frame and ψ = [ ψ  ψ  ψ  ] as the Euler angle rates, then we have their relationship with the Euler angle: Without linearization, the attitude error differential equation can be derived from (7) as where C  is the transition matrix between -frame and  frame [17].
Based on (8), the attitude, the velocity, and the position error equations are expressed as where ω  is the computed angular rotation velocity of the navigation coordinate system with respect to the inertial frame; ω  is the computed earth rotation velocity; ω  is the computed rotation vector from the -frame to the -frame; their errors are    ,    , and    ; f  is the specific force that the accelerators measure; k , L, λ, and h and , , ℎ, and k  are the computed velocity, latitude, longitude, and height and their errors, respectively; R , R and   ,   are the radius of the meridian and the prime vertical circle and their errors, respectively [18].
And the SINS error model is written as 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.

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]: where F() is the coefficient matrix of the state equation; Δ is the filter cycle; q is the covariance matrix of SINS sensors; and G() is the coefficient matrix of the state noise.For a loosely coupled SINS/GNSS system, the measurement equation is [20] where z is the difference of the velocity and position between SINS and GPS; H = [I 6×6 0 6×9 ]; k is the noise standard deviation vector.KF algorithm is committed by using such formulas [21]: In SINS/GPS integration, a close-loop configuration is used.In every KF epoch when the SINS errors are corrected, the state x is set to be zero.

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: where  is the dimension of the state vector x;  =  2 ( + ) − , which decides the weights of the distribution of the sigma points and, generally,  = 3 −  while 10 −4 ≤  ≤ 1.And (√( + )P −1 ) () is the  column of the Cholesky decomposition of the matrix ( + )P −1 .
Finally, the states are weighted and summarized so we obtain the predicted state x|−1 and the error covariance matrix P |−1 : |−1 , where Q −1 is the covariance matrix of the state noise;  ()  and  ()  are the weights of the sigma points, which could be calculated as 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|−1 and the error covariance matrix P |−1 , we could estimate the SINS errors with formulas (17) to (19).

Hybrid KF-UKF Algorithm.
In this part we discussed the architecture of the hybrid KF-UKF algorithm.As shown in Figure 1, 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.
If GPS information is available, SINS and GPS can be integrated by normal KF method, as shown in Figure 2. First, linear SINS error equation ( 6) is discretized by formula (13) with SINS information at time  =  − 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  =  are estimated with GPS information at  = .Finally, the SINS error estimated at  =  is feedback to SINS, correcting the SINS error, and the same procedure is committed in the next cycle.

SINS update
One-step prediction:  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 Figure 3, let us assume that the GPS signal breaks off at  =  and recovers at  =  + .In order to estimate SINS error at  =  + , it is necessary to predict the state x+|−1 and error covariance P +|−1 .Suppose that  +  is a time instant in GPS outage, and x+|−1 , P +|−1 are iterated Error covariance: P k+n   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.

Simulation Results and Analysis.
In the simulation test, the aircraft has done a series of maneuvers which are listed in Table 2 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. at  = 270 s.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  = 270 s, 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.Table 3 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 C    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 ,   , and   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 Table 4, 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 ( × ) dimension matrix to another ( × ) dimension matrix, then  flops have been generated in the computer.In Table 4, we suppose the dimension of the state vector is  and the measurement vector dimension is .During the prediction updating period, 4 3 + 2 − flops are generated in EKF while 6(2/3) 3 +12 2 + 3 flops are generated in UKF.In SINS/GNSS integration,  = 15 and  = 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

Figure 2 :
Figure 2: Filter updates procedure when GPS is functioning well.

Figure 4 :
Figure 4: Schematic diagram of the simulation.

Table 1 :
Main specifications of experiment instruments in the simulation test.

Table 2 :
Aircraft maneuvers in the test.

Table 3 :
Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage.