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.
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:
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 aswhere is the direction cosine matrix (DCM) from the body-frame (-frame) to the computed navigation frame (-frame); is the DCM from -frame to the real navigation frame (-frame); is the DCM from -frame to -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
The attitude, the velocity, and the position error equations, respectively, arewhere 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; is the specific force vector in -frame; is the error of the gravity vector in -frame; and 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.
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 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) aswhere is the transition matrix between -frame and -frame [17].
Based on (8), the attitude, the velocity, and the position error equations are expressed aswhere 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 ; is the specific force that the accelerators measure; , , , and and , , , and are the computed velocity, latitude, longitude, and height and their errors, respectively; , 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.
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]:where is the coefficient matrix of the state equation; is the filter cycle; is the covariance matrix of SINS sensors; and is the coefficient matrix of the state noise.
For a loosely coupled SINS/GNSS system, the measurement equation is [20]where is the difference of the velocity and position between SINS and GPS; ; 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 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:where is the dimension of the state vector ; , which decides the weights of the distribution of the sigma points and, generally, while . And is the column of the Cholesky decomposition of the matrix .
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:
Finally, the states are weighted and summarized so we obtain the predicted state and the error covariance matrix :where is the covariance matrix of the state noise; and are the weights of the sigma points, which could be calculated aswhere and have been given in the first step and is assigned according to the distribution character of the state error. In this case, .
In loosely coupled SIN/GPS, the measurement equation is linear as we saw in (14). So after we predict the state and the error covariance matrix , 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 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 . 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.
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 and error covariance . Suppose that is a time instant in GPS outage, and are iterated by using and SINS information at time , , and with formulas (24) to (27). This calculation is executed cycle by cycle until GPS signal recovers at . When GPS receiver offered the velocity and position information of the vehicle at , we already have the state and error covariance . Then we can estimate SINS error at with formulas (18) and (19), correct it, and go into the next filter cycle
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 Figure 4. 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.
4.2. 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.
Figure 5 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 and recovered at . 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 , 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 and 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, flops are generated in EKF while flops are generated in UKF. In SINS/GNSS integration, and . 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 () 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 , 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.
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.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This research is supported by the 3rd Innovation Fund of Changchun Institute of Optics, Fine Mechanics and Physics (CIOMP) and the UAV semi-physical simulation platform research project, Chinese Science Academy (CSA).