Adaptive Estimation of Multiple Fading Factors for GPS/INS Integrated Navigation Systems

The Kalman filter has been widely applied in the field of dynamic navigation and positioning. However, its performance will be degraded in the presence of significant model errors and uncertain interferences. In the literature, the fading filter was proposed to control the influences of the model errors, and the H-infinity filter can be adopted to address the uncertainties by minimizing the estimation error in the worst case. In this paper, a new multiple fading factor, suitable for the Global Positioning System (GPS) and the Inertial Navigation System (INS) integrated navigation system, is proposed based on the optimization of the filter, and a comprehensive filtering algorithm is constructed by integrating the advantages of the H-infinity filter and the proposed multiple fading filter. Measurement data of the GPS/INS integrated navigation system are collected under actual conditions. Stability and robustness of the proposed filtering algorithm are tested with various experiments and contrastive analysis are performed with the measurement data. Results demonstrate that both the filter divergence and the influences of outliers are restrained effectively with the proposed filtering algorithm, and precision of the filtering results are improved simultaneously.


Introduction
With a full solution for position, velocity, and attitude, the GPS and INS systems are both effective navigation techniques which have found broad applications. GPS, characterized by high precision, is able to provide accurate navigation and positioning information. INS is a self-navigation system which needs not to send and receive signals. The GPS/INS integrated navigation system manifests great superiority by integrating the advantages of GPS and INS, and performance of the integrated system is better than that of either the GPS or INS system alone [1]. Thus, the integration of GPS and INS gives a more complete and accurate navigation solution, and this integrated system has been widely adopted in the field of dynamic navigation and positioning in recent years. The Kalman filter is the most popular real-time optimal estimator [2], and it has been commonly applied in navigation applications. In the GPS/INS integrated navigation system, the Kalman filter is also the most commonly used data fusion method [3][4][5]. The parameter estimates of the Kalman filter are optimal when the statistical information of the noise is correctly specified. However, the required information is difficult to achieve when the model errors have time varying characteristics [6,7], and the performance will be degraded with improper statistical information and outliers, and sometimes may even lead to a filter divergence [7]. Moreover, it is unavoidable, especially when caused by a complex perturbation in a signal degraded environment [3]. As one of the basic procedures for the quality control of the Kalman filter, the Detection, Identification and Adaptation (DIA) methods were proposed to

Basic Rules of the Fading Filter
For the dynamic model equation and the measurement equation: the predicted state vector is presented as: where x k denotes the state vector at the epoch k, Φ k,k−1 denotes the state transition matrix, H k denotes the measurement matrix, z k denotes the measurement vector, w k and v k denote the state noise and measurement noise, respectively, and x k/k−1 denotes the predicted state vector. The recursion approach of the fading filter is given by: where K k denotes the equivalent gain matrix, P k/k−1 denotes the equivalent covariance matrix of the state vector, S k denotes the fading factor and S k ≥ 1, P k−1 denotes the covariance matrix of the state vector at epoch k − 1, R k and Q k denote the covariance matrices of the measurement noise and the state noise, respectively. In the standard Kalman filter, the covariance matrix of the state vector is given by: In the fading filter, the covariance matrix of the a prior state is inflated for S k times which degrades the efficiency of the past state information, and the recent measurement information is regarded more seriously [12]. Obviously, compared with the standard Kalman filter, the state errors from the previous epoch are well controlled.

The Recent Fading Factors
The key problem in the fading filter is to find a suitable fading factor. With the optimization considered, a fading filter algorithm was proposed [14] where two forms of fading factors were derived, namely, the steepest descent method and the one-step method. It is quite difficult to apply the former method due to its complex computations. In practice, the latter is more applicable which is derived by: where: where tr(·) denotes taking trace of a matrix, V k denotes the predicted residual vector, and P V k denotes [12]. Then Xia [14] proposed a simplified method, namely: the fading factor will increase along with V k , and S k calculated in this way is optimal in theory. A multiple fading filter algorithm was proposed by Geng [19], and the fading factor is given by: .., s t , 1, ..., 1, ..., 1), where k , ε i is the threshold value fixed according to the Chi-square distribution at the given confident level. In this approach, only (s 1 , s 2 , ..., s t ) can be estimated adaptively due to their observabilities, and the other elements should be fixed as one.

Principles of the H-Infinity Filter
Assume that the dynamic model equation and the measurement equation is given by: where y k denotes the state vector to be estimated, L k denotes the given matrix, and L k is often fixed by an identity matrix. No statistical information of the noises is assumed before the filtering. The cost function J is defined by [21]: where N denotes the total number of filtering time limit, x 0 denotes the initial value of the state vector x with the covariance matrix P 0 ,x 0 andx k denote the estimated state vectors of x 0 and x k , respectively, and the expression . The estimate of the state vector should be achieved under the conditions ofx k = arg min J ∞ . However, the closed-form approach of an optimal H-infinity filter can be achieved only under special conditions [23]. Accordingly, a suboptimal recursion algorithm was proposed through setting a threshold value γ which satisfied the following Riccati inequality [1]: where P k is the covariance matrix of x k . Then, the recursion formulas of the H-infinity filter in the linear systems, which are different with the standard Kalman filter, are obtained [24]: It should be noticed that the robustness of the filter is related to the threshold value γ, and the filter becomes more robust when γ decreases. However, it may cause a filter divergence if γ is too close to zero [23].

Adaptive Estimation of the Multiple Fading Factor
In this section, the adaptive estimation of the new multiple fading factor is proposed, and a multiple fading filtering algorithm is constructed with the proposed multiple fading factor. Finally, the recursion formulas of the new algorithm are derived.
As mentioned above, the performance of the Kalman filter may degrade due to the past information. A constant fading factor was thus designed to address this problem [14]. However, a constant fading factor cannot guarantee optimal filtering absolutely. A multiple fading factor was proposed to improve the performance of a single fading factor [19], whereas it is unsatisfactory with respect to the robustness of the filtering. Accordingly, a new multiple fading filter is proposed to further improve the performance of the fading filter.
For the a prior covariance matrix in the multiple fading filter: S k = diag(s 1 , s 2 , ..., s t , 1, 1, ..., 1). To keep the symmetry of P k/k−1 , the Equation (20) can be rewritten as: Based on the observability and the optimal fading factor, a new multiple fading factor is constructed as: where N k (i, i) and M k (i, i) represent the i − th diagonal elements of N k and M k , respectively. Similarly, only (s 1 , s 2 , ..., s t ) can be estimated adaptively due to their observabilities, and the other elements should be fixed as one. The multiple fading factors in Equation (22) are constructed based on the optimal single fading factor in Equation (12) and they can guarantee the optimization of different data channels, thus the optimization of the whole filter is guaranteed. Then, a new multiple fading H-infinity filtering algorithm is constructed. With the H-infinity filter adopted, the recursion formulas of the new algorithm for a linear system are listed below (the predicted state vector is calculated in the same way as Equation (2)): where P k/k denotes the a posteriori covariance matrix of the state, and S k is fixed through (22).

The GPS/INS Integrated Navigation System
In this section, the dynamic model and the state estimator are listed. Then the loosely-coupled GPS/INS integrated navigation system is discussed in detail.
In recent years, three main forms of integration are developed: the loosely-coupled, the tightly-coupled and the deeply-coupled. Compared with the other two forms of integration, the loosely-coupled navigation system is easier to be implemented and the computation process is more concise [25,26].
A 15-dimension state vector is designed in the loosely-coupled GPS/INS integrated navigation system in this paper. And the parameters denote the deviations of position, velocities, attitudes, and the random bias of the gyroscope and accelerometer, respectively. The state vectorX is given by: The nonlinear differential error model for the INS system is defined by [6,27]: where ∆R e and ∆V e are the position and velocity errors under the computer e frame, respectively, . φ e is the attitude error between e frame and the platform e frame, I 3×3 is the identity matrix, C e e is the rotation matrix between e and e frame, C e b is the rotation matrix between the body frame and e frame, Ω e ie is the skew symmetric matrix of earth rotation rate ω e ie , ∇ b and ε b are the gyroscope and accelerometer drift errors under the body frame, respectively.
In this paper, the cubature Kalman filter is adopted to address the nonlinear problem of the integrated navigation system. For a discrete nonlinear system: where f (·) and h(·) denote the known nonlinear functions. The cubature points are generated by: where m denotes the number of the cubature points and m = 2n, n denotes the dimension of the state vector, [1] denotes the integral and symmetrical set of points. Then equations of the cubature Kalman filter are given by [28].
then the final measurement update equations are obtained: where s denotes the square root of the covariance matrix P, X k−1,k−1 denotes the cubature points of the states vector, X * k/k−1 denotes the propagated cubature points, SVD denotes the singular value decomposition of a matrix; Z k/k−1 denotes the propagated cubature points of the measurement vector. Accordingly, the multiple fading filtering algorithm for the GPS/INS integrated navigation system is listed below: In the loosely-coupled GPS/INS integrated navigation system, the differences of position and velocity between GPS and INS are regarded as the measurement inputs to the integration algorithm, namely: where ρ GPS and ρ INS are the position and velocity information output by the GPS and INS, respectively. Then the measurement vector is presented as: where Z k denotes the measurement vector of the integrated navigation system at epoch k, r GPS , r INS , v GPS , and v INS are the position and velocity information of GPS and INS, respectively. In the loosely-coupled navigation system, the measurement equation is linear, then the Equation (40) can be rewritten as: Obviously, the observable variables are the position and velocity information of each direction, thus the corresponding fading factors can be estimated adaptively.

Testing
In this section, two cases of a loosely-coupled GPS/INS integrated navigation system are designed and implemented. Then the performance of the proposed filtering algorithm is discussed in comparison with the existing algorithms.

Experiment Schemes
To examine the performance of the proposed multiple fading filter, various actual experiments were performed. The data were obtained through a vehicle mounted GPS/INS integrated navigation system. In this system, two Trimble GPS receivers and a low cost inertial measurement unit (IMU) were adopted, and the main parameters of the IMU are listed in Table 1. The update rate of GPS and IMU were set by 1 Hz and 100 Hz, respectively. The initial position and velocity errors are 1.0 m and 0.1 m/s, respectively. The initial position errors are set by 3.0 m, 3.0 m, 5.0 m, respectively, the initial velocity error is 0.5 m/s, and the initial alignment errors are set by 3 • , 3 • , 10 • , respectively. Double difference GPS data were adopted with the position and velocity variances of 0.25 m 2 and 0.0025 m 2 /s 2 , respectively. The precise results calculated by the double difference carrier phase were regarded as references. The motion trajectory of the land vehicle is displayed in Figure 1.
Then, two cases were designed to evaluate the stability and robustness of the new algorithm: the original measurements were adopted in case 1, and the measurements with perturbations in case 2. In each case, six schemes were performed:                     Big disturbances are found in Figure 2 which may occur when the vehicle is passing through the speed hump, and this indicates that the robustness of the CKF algorithm should be strengthened. It is concluded from Figures 2 and 3 that, with the effects of the uncertainties controlled, the HF-CKF algorithm performs much better than the CKF algorithm. Compared with the CKF algorithm, Figures  4 and 5 demonstrate that both the conventional and the new multiple fading filters manifest certain ability of controlling the influence of the model deviations. By integrating the advantages of the multiple fading filter and the H-infinity filter, the MHF-CKF and NMHF-CKF algorithms show better performance than the other algorithms which can be learned from the Figures 6 and 7, and this indicates that the influences of the model deviations and uncertain interferences are well controlled with the MHF-CKF and NMHF-CKF algorithms. In Table 2, RMSEs of the MF-CKF and NMF-CKF algorithms are much smaller than those of the CKF algorithm, and it can be concluded that the fading factors have reduced the weight of the past state information. Consequently, compared with the conventional Kalman filter, the impacts of the state model errors are controlled effectively with the multiple fading filter algorithm, and thus a higher precision is obtained. Figure 7 and Table 2 demonstrate that the new multiple fading filter performs the best among the six schemes, and both the amplitudes of errors and the RMSEs are further reduced. As displayed in Table 3, there exists a little differences of the attitude errors between each algorithm, and the RMSEs of the NMHF-CKF algorithm are slightly smaller than those of the other algorithms.

Results of the Experiments
(2) Case 2:   Table 3. Attitude RMSEs of each scheme (degree). Big disturbances are found in Figure 2 which may occur when the vehicle is passing through the speed hump, and this indicates that the robustness of the CKF algorithm should be strengthened. It is concluded from Figures 2 and 3 that, with the effects of the uncertainties controlled, the HF-CKF algorithm performs much better than the CKF algorithm. Compared with the CKF algorithm, Figures 4 and 5 demonstrate that both the conventional and the new multiple fading filters manifest certain ability of controlling the influence of the model deviations. By integrating the advantages of the multiple fading filter and the H-infinity filter, the MHF-CKF and NMHF-CKF algorithms show better performance than the other algorithms which can be learned from the Figures 6 and 7, and this indicates that the influences of the model deviations and uncertain interferences are well controlled with the MHF-CKF and NMHF-CKF algorithms. In Table 2, RMSEs of the MF-CKF and NMF-CKF algorithms are much smaller than those of the CKF algorithm, and it can be concluded that the fading factors have reduced the weight of the past state information. Consequently, compared with the conventional Kalman filter, the impacts of the state model errors are controlled effectively with the multiple fading filter algorithm, and thus a higher precision is obtained. Figure 7 and Table 2 demonstrate that the new multiple fading filter performs the best among the six schemes, and both the amplitudes of errors and the RMSEs are further reduced. As displayed in Table 3, there exists a little differences of the attitude errors between each algorithm, and the RMSEs of the NMHF-CKF algorithm are slightly smaller than those of the other algorithms.

Axis CKF HF-CKF MF-CKF NMF-CKF MHF-CKF NMHF-CKF
(2) Case 2: In this case, ability of robustness for the above schemes is examined with both scattered and continuously-changing outliers. Thus the scattered outliers at the 160th, 260th, 360th, 460th, 560th epochs, respectively, and the continuously-changing outliers were added artificially to the GPS measurements at the 351th to the 380th epochs. Then the six schemes were implemented with the new data. Position errors of these schemes are displayed in Figures 8-13: 17, 1254 13 of 18 In this case, ability of robustness for the above schemes is examined with both scattered and continuously-changing outliers. Thus the scattered outliers at the 160th, 260th, 360th, 460th, 560th epochs, respectively, and the continuously-changing outliers were added artificially to the GPS measurements at the 351th to the 380th epochs. Then the six schemes were implemented with the new data. Position errors of these schemes are displayed in Figures 8-13:  In this case, ability of robustness for the above schemes is examined with both scattered and continuously-changing outliers. Thus the scattered outliers at the 160th, 260th, 360th, 460th, 560th epochs, respectively, and the continuously-changing outliers were added artificially to the GPS measurements at the 351th to the 380th epochs. Then the six schemes were implemented with the new data. Position errors of these schemes are displayed in Figures 8-13:       In this case, the position errors are mainly caused by outliers. It is clearly demonstrated from the Figures 8 and 9 that the CKF and HF-CKF algorithms show little ability to resist the outliers. On the contrary, both the conventional and the new fading filters manifest a noticeable robustness which can be seen from the Figures 10-13. By integrating the advantages of the H-infinity filter, filtering precision is furtherly improved with the MHF-CKF and the NMHF-CKF algorithms. In addition, error amplitudes of the new multiple fading filter are smaller than those of the other filtering algorithms, which indicates that effects of both scattered and continuously-changing outliers are weakened effectively. Then the position and attitude RMSEs of each scheme are listed in Tables 4 and 5:   0  100  200  300  400  500  600  700   In this case, the position errors are mainly caused by outliers. It is clearly demonstrated from the Figures 8 and 9 that the CKF and HF-CKF algorithms show little ability to resist the outliers. On the contrary, both the conventional and the new fading filters manifest a noticeable robustness which can be seen from the Figures 10-13. By integrating the advantages of the H-infinity filter, filtering precision is furtherly improved with the MHF-CKF and the NMHF-CKF algorithms. In addition, error amplitudes of the new multiple fading filter are smaller than those of the other filtering algorithms, which indicates that effects of both scattered and continuously-changing outliers are weakened effectively. Then the position and attitude RMSEs of each scheme are listed in Tables 4 and 5:   0  100  200  300  400  500  600  700  In this case, the position errors are mainly caused by outliers. It is clearly demonstrated from the Figures 8 and 9 that the CKF and HF-CKF algorithms show little ability to resist the outliers. On the contrary, both the conventional and the new fading filters manifest a noticeable robustness which can be seen from the Figures 10-13. By integrating the advantages of the H-infinity filter, filtering precision is furtherly improved with the MHF-CKF and the NMHF-CKF algorithms. In addition, error amplitudes of the new multiple fading filter are smaller than those of the other filtering algorithms, which indicates that effects of both scattered and continuously-changing outliers are weakened effectively. Then the position and attitude RMSEs of each scheme are listed in Tables 4 and 5: In Table 4, position RMSEs of the CKF and the HF-CKF algorithms are significantly affected, and the HF-CKF algorithm exerts certain robustness compared with the CKF algorithm. Position RMSEs of the conventional and the new multiple fading filtering algorithms change little in the presence of outliers, and the new multiple fading filter shows a higher accuracy than the conventional multiple fading filter whether the H-infinity filter is ever adopted. The Table 5 indicates that the proposed algorithm performs better than the other algorithms, and the influence of the attitude errors are weakened further. Due to the fading factors, multiple data channels are adjusted depending on their observabilities of the state vector, and the state model errors from the previous epoch are well controlled, thus the filter becomes more stable and the filter divergence caused by the model deviations is restrained. It should be pointed out however, in practice, N k may sometimes become negative definite and S k has to be fixed by 1 now, and this indicates that the state model error may not be controlled effectively at the current epoch [12].

Conclusions
In this study, a new multiple fading factor is proposed based on the thought of fading memory and the optimization of the filter, and a comprehensive filtering algorithm is constructed by integrating the proposed multiple fading filter and the H-infinity filter. The new filtering algorithm is implemented in the loosely-coupled GPS/INS integrated navigation system. The detail conclusions in this paper are summarized below: (1) The H-infinity filter based on the cubature Kalman filter performs better than the cubature Kalman filter, however, they are both affected significantly by the model errors and outliers.
The conventional multiple fading filter shows certain robustness which can be furtherly enhanced. (2) With the influences of the model deviations and the uncertain interferences controlled simultaneously, performance of the conventional multiple fading filter is further improved, and the higher precision is obtained by integrating the advantages of the multiple fading filter and the H-infinity filter. The proposed multiple fading filter is implemented in the loosely-coupled GPS/INS integrated navigation system, and the stability and robustness are demonstrated with the original data and the data with both the scattered and the continuously-changing outliers.