A New Adaptive H-Infinity Filtering Algorithm for the GPS/INS Integrated Navigation

The Kalman filter is an optimal estimator with numerous applications in technology, especially in systems with Gaussian distributed noise. Moreover, the adaptive Kalman filtering algorithms, based on the Kalman filter, can control the influence of dynamic model errors. In contrast to the adaptive Kalman filtering algorithms, the H-infinity filter is able to address the interference of the stochastic model by minimization of the worst-case estimation error. In this paper, a novel adaptive H-infinity filtering algorithm, which integrates the adaptive Kalman filter and the H-infinity filter in order to perform a comprehensive filtering algorithm, is presented. In the proposed algorithm, a robust estimation method is employed to control the influence of outliers. In order to verify the proposed algorithm, experiments with real data of the Global Positioning System (GPS) and Inertial Navigation System (INS) integrated navigation, were conducted. The experimental results have shown that the proposed algorithm has multiple advantages compared to the other filtering algorithms.


Introduction
Accurate Global Positioning System (GPS) measurement data can be used to control the Inertial Navigation System (INS) accumulative errors. On the other hand, the INS can be used in cases when the GPS fails. Thus, the integration of GPS and INS has been widely adopted in the field of dynamic navigation and positioning. The Kalman filter is one of the most celebrated real-time optimal estimators [1], and therefore it has become the basic data-fusion approach in navigation and positioning [2,3]. Unfortunately, the Kalman filter performance depends on many factors, thus it cannot meet the requirements of certain nonlinear systems and the performance is susceptible to outliers. Hence, many improved filtering algorithms were proposed, for instance, the unscented Kalman filter [4,5], the particle filter [6], the robust filter [7,8], the adaptive filter [9][10][11][12], and so forth. The nonlinear problem can be handled by the unscented Kalman filter or the particle filter, however, they both become unstable in a high-dimensional case. On the other hand, the cubature Kalman filter is a recently developed nonlinear filtering algorithm [13]. Compared with the unscented Kalman filter, the cubature Kalman filter shows better performance in stability; therefore, it has been adopted in the GPS/inertial measurement unit (IMU) integrated navigation system [14]. Many forms of robust filtering algorithms were proposed to control the influence of outliers, but the influence of the model errors was neglected. The multiple-model-based adaptive estimation (MMAE) and innovation-based adaptive estimation (IAE) represent the adaptive Kalman filtering approaches. In the MMAE, there is a set of Kalman filters which work in parallel under different models, while in the IAE the adaptation is applied directly to the statistical information. Moreover, the IAE is more applicable two adaptive factors were derived, and the filtering performance of the algorithm based on the optimal adaptive factor was superior to the one based on the nonoptimal adaptive factors.
The dynamic model and measurement model are given by: where x k is the state vector, Φ k,k−1 presents the state transition matrix, H k is the measurement matrix, z k denotes the measurement vector, and w k and v k are the system noise and measurement noise, respectively. The predicted state vector and its covariance matrix are defined by: where x k/k−1 is the predicted state vector, and P k/k−1 and Q k denote the predicted covariance matrices of x k/k−1 and w k , respectively. According to the rule of the least squares estimation, the following can be written: where V k and V x k/k−1 denote the residual vectors of z k and x k/k−1 , respectively. The solution of the standard Kalman filter is obtained by: where R k is the covariance matrix of the measurement noise. If the risk function is defined by: then the solution of the adaptive Kalman filter is obtained by: where α k denotes the adaptive factor. The equivalent expression of Equation (8) is in reference [27]: Four types of the adaptive factors and error-learning statistics were summarized in reference [17]. The predicted residual statistic was adopted if no redundant information exists, and the adaptive factor based on the three-segment function was [16,29]: where ∆V k denotes the learning statistic based on the predicted residual, , P V k = H k P k/k−1 H T k + R k , V k denotes the innovation vector, further, 1.0 ≤ c 0 ≤ 1.5, and 3.0 ≤ c 1 ≤ 8.5. In Equation (9), α k = 0, therefore, when Equation (9) is used, the adaptive factor based on the two-segment function [17], defined by Equation (11), should be adopted: where 1.0 ≤ c ≤ 2.5, and the optimal value of c is 1.0 [12,17].

Basic Principle of the H-Infinity Filter
The H-infinity filter is a special form of the Kalman filter, and the principle of the H-infinity filter is based on the H-infinity optimal estimation that guarantees the smallest estimation energy error for all possible disturbances of the fixed energy [30]. The cost function of a nonlinear filter is defined by: where N is the number of filtering epochs, x 0 is the initial value of the state vector x with the covariance matrix P 0 ,x 0 andx k are the estimated state vectors of x 0 and x k , respectively, and the expression In general, the estimate,x k , which satisfieŝ x k = arg min J ∞ , should be found in order to minimizes J. Actually, an analytical solution for the optimal H-infinity filter is difficult to achieve. Thus, a suboptimal recursion algorithm is developed by setting a threshold value, γ, which satisfies the following Riccati inequality [31]: where P k is the covariance matrix of x k , L k is the coefficient matrix of the constraint equation, and L k is generally defined by the unit matrix I. The recursion equations of the H-infinity filter are listed below [31]: The H-infinity filter minimizes the estimation error in the worst case, which makes it more robust than the standard Kalman filter. In addition, the H-infinity filter becomes more robust when the constraint parameter γ decreases. However, the value of γ should not be in the vicinity of zero, because that might cause the divergence of the H-infinity filter [32]. In general, γ is fixed to certain value, which is chosen by experience. In addition, the H-infinity filter represents a rigorous method for the system with an unreliable model [30].

A Novel Adaptive H-Infinity Filtering Algorithm
In the adaptive filtering algorithms, it is assumed that the measurements are reliable and that the predicted residual can reflect the deviations of the model information. However, some of the adaptive filtering algorithms cannot control the effects of the outliers. As mentioned before, the H-infinity filter can overcome the uncertainties in the measurements, but the effects of the outliers cannot be controlled just by the H-infinity filter, thus, some robust Kalman filtering algorithms were developed [7].
In this paper, an integrated adaptive H-infinity filtering algorithm is proposed. An adaptive factor was constructed in order to control the influence of the dynamic model errors, and the H-infinity filter was adopted to address the uncertain interference. The recursion equations of the adaptive H-infinity filter can be expressed by: where K k denotes the equivalent gain matrix of the H-infinity filter. Since, there were no redundant measurements, the adaptive factor based on the two-segment function was adopted, and the predicted residual was selected as the statistic value. In the integrated adaptive H-infinity filtering algorithm, the robust estimation method was employed to decrease the effects of outlying measurements. The selection of the equivalent weight and equivalent covariance matrix should be considered in the robust estimation. In general, the components of the predicted state vector are correlated, so the equivalent covariance matrix constructed by scaling factor, λ ij , is applied. The scaling factor is defined by reference [33]: where V X k i denotes the component of the predicted residual vector, 1.0 ≤ c ≤ 1.5, and λ jj can be expressed the same as λ ii . Thus, the equivalent covariance matrix R k is obtained by The entire process of the proposed filtering algorithm is shown in Figure 1.
The entire process of the proposed filtering algorithm is shown in Figure 1.

The GPS/INS Integrated Navigation
Recently, in the GPS/INS integrated navigation, three types of integration have been developed: loosely coupled, tightly coupled, and deeply coupled [34]. In the loosely coupled integrated navigation, a 15-dimension state vector is adopted, which includes the deviations of position, velocities, attitudes, and the noises of gyroscope and accelerometer. Thus, the state vector x is defined by: Since, the GPS/INS integrated navigation system represents a nonlinear system, the cubature Kalman filter is applied to address the nonlinear problem. For a discrete nonlinear system, it can be written as follows:

The GPS/INS Integrated Navigation
Recently, in the GPS/INS integrated navigation, three types of integration have been developed: loosely coupled, tightly coupled, and deeply coupled [34]. In the loosely coupled integrated navigation, a 15-dimension state vector is adopted, which includes the deviations of position, velocities, attitudes, and the noises of gyroscope and accelerometer. Thus, the state vectorx is defined by: Since, the GPS/INS integrated navigation system represents a nonlinear system, the cubature Kalman filter is applied to address the nonlinear problem. For a discrete nonlinear system, it can be written as follows: x 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, and [1] denotes the point 1 0 in this paper. Then, the equations of the cubature Kalman filter are given by reference [14]: then the final measurement update equations are obtained by: 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; and Z k/k−1 denotes the propagated cubature points of the measurement vector. Accordingly, the adaptive H-infinity filtering algorithm intended for the GPS/INS integrated navigation system is summarized as follows: (a) Time update In addition, R k should be replaced with the equivalent covariance matrix R k in order to make the algorithm more robust.
In the loosely coupled integrated navigation, the differences in position and velocity, Z ρ , between GPS and INS are considered as the external measurements, namely: where Z k represents the measurement vector of coupled, the tightly coupled integration has better performance in terms of precision. However, the loosely coupled navigation can be implemented easily, and the computation process is more concise [35].

Test Case 1
In this case, the GPS and INS data were obtained by a small aerial vehicle equipped with inertial sensors of automotive-grade quality and a GPS receiver [36]. Four schemes were implemented to examine the performance of the proposed filtering algorithm: Position errors of all mentioned schemes are demonstrated in the following: As it can be seen in previous figures, there is little difference between Figures 2 and 3, which indicates that there are limited effects of the model errors. Due to the minimization of the worst-case estimation error, the H-infinity filter performs slightly better than the adaptive Kalman filter, which can be seen from Figure 4 and Table 1. Nonetheless, Figure 5 indicates that a noticeable improvement was obtained by the adaptive H-infinity filtering algorithm in terms of dynamic model errors and the uncertain interferences. Position errors of all mentioned schemes are demonstrated in the following: As it can be seen in previous figures, there is little difference between Figures 2 and 3, which indicates that there are limited effects of the model errors. Due to the minimization of the worst-case estimation error, the H-infinity filter performs slightly better than the adaptive Kalman filter, which can be seen from Figure 4 and Table 1. Nonetheless, Figure 5 indicates that a noticeable improvement was obtained by the adaptive H-infinity filtering algorithm in terms of dynamic model errors and the uncertain interferences.         The Root Mean Squares Errors (RMSEs) of the schemes are presented in Table 1.  The Root Mean Squares Errors (RMSEs) of the schemes are presented in Table 1.

Test Case 2
In this experiment, data were collected by a vehicle equipped with a GPS/INS integrated navigation system, which was composed of two GPS receivers and an inertial measurement unit (IMU). One of the GPS receivers was set as a reference station, and another receiver as well as the IMU were mounted on the vehicle. GPS data were calculated by the double difference pseudorange with variances of 0.25 m 2 and 0.0025 m 2 /s 2 , respectively. The sampling frequencies of GPS and IMU were 1 Hz and 100 Hz, respectively. The precise results achieved by the double difference carrier phase were regarded as references. Afterwards, four schemes were performed in the integrated navigation system: Scheme 1: Kalman Filter (KF); Scheme 2: Adaptive Kalman Filter (AKF) (two-segment function was adopted and c = 1.0); Scheme 3: H-infinity Filter (HF); Scheme 4: Adaptive H-infinity Filter (AHF) (two-segment function was adopted and c = 1.0, and the robust estimation method was adopted with the double-factors where c = 1.5).

Experiments without Outliers
Position errors of used schemes are demonstrated in the following: Due to the vehicle's movement over the bumps, disturbances can be found in both Figures 6  and 7, which indicates that the robustness of the KF and AKF algorithms should be improved. Because of decrease of the dynamic model errors, the AKF algorithm shows a slightly better performance than the KF algorithm. Apparently, the uncertain interference is controlled, thus, the HF and AHF algorithms (Figures 8 and 9) perform much better than the first two algorithms. The robustness of the AHF algorithm is improved by the robust estimation method, therefore, the error amplitude is reduced. The RMSE of each scheme is given in Table 2.
algorithms (Figures 8 and 9) perform much better than the first two algorithms. The robustness of the AHF algorithm is improved by the robust estimation method, therefore, the error amplitude is reduced. The RMSE of each scheme is given in Table 2.
As mentioned before, RMSEs of the AKF and HF algorithms, presented in Table 2, are both smaller than RMSE of the KF algorithm. The integration of the AKF and HF algorithms into the AHF algorithm provides better performance compared to the other algorithms.       As mentioned before, RMSEs of the AKF and HF algorithms, presented in Table 2, are both smaller than RMSE of the KF algorithm. The integration of the AKF and HF algorithms into the AHF algorithm provides better performance compared to the other algorithms.

Experiments with Outliers
In these experiments, we used data from the Section 5.2.1, but the single gross errors were added artificially to the GPS measurement data at the 160th, 360th, 460th, and 560th epochs, respectively, and the constantly changed outliers were added to all epochs between the 251th and 280th epochs. Then, the previously used schemes were implemented again, and the obtained position errors of all schemes are displayed in Figures 10-13.  In these experiments, we used data from the Section 5.2.1, but the single gross errors were added artificially to the GPS measurement data at the 160th, 360th, 460th, and 560th epochs, respectively, and the constantly changed outliers were added to all epochs between the 251th and 280th epochs. Then, the previously used schemes were implemented again, and the obtained position errors of all schemes are displayed in Figures 10-13.
According to the presented results, it can be concluded that in the presence of outliers, filtering results were mostly dependent on outliers. Moreover, Figures 10 and 11 show that the KF and AKF filtering results have similar trends, while the latter figure manifests a reduced error. As it can be seen in Figure 12, the filtering results of the HF algorithm are stable except in the epochs that contain outliers. Furthermore, Figure 13 shows that the proposed filtering algorithm has better performance than other algorithms, so the influence of single and constantly changed outliers is reduced using the AHF algorithm. The RMSE values of all used scheme are presented in Table 3.
Compared to the Kalman filter, the filtering precision of the AKF and HF algorithms is improved. However, both AKF and HF algorithms are significantly affected by the outliers. On the other hand, the AHF algorithm shows better fault tolerance and robustness compared to other schemes. The adaptive H-infinity filter is more robust because of the robust estimation method, based on the control of dynamic model errors and uncertain interference. In all presented cases, RMSEs of the AHF algorithm are the smallest for all coordinates, which means that the positions calculated by the AHF algorithm are in good agreement with the actual positions.             According to the presented results, it can be concluded that in the presence of outliers, filtering results were mostly dependent on outliers. Moreover, Figures 10 and 11 show that the KF and AKF filtering results have similar trends, while the latter figure manifests a reduced error. As it can be seen in Figure 12, the filtering results of the HF algorithm are stable except in the epochs that contain outliers. Furthermore, Figure 13 shows that the proposed filtering algorithm has better performance than other algorithms, so the influence of single and constantly changed outliers is reduced using the AHF algorithm. The RMSE values of all used scheme are presented in Table 3. Compared to the Kalman filter, the filtering precision of the AKF and HF algorithms is improved. However, both AKF and HF algorithms are significantly affected by the outliers. On the other hand, the AHF algorithm shows better fault tolerance and robustness compared to other schemes. The adaptive H-infinity filter is more robust because of the robust estimation method, based on the control of dynamic model errors and uncertain interference. In all presented cases, RMSEs of the AHF algorithm are the smallest for all coordinates, which means that the positions calculated by the AHF algorithm are in good agreement with the actual positions.

Conclusions
An integrated adaptive H-infinity filtering algorithm for mitigation of positioning errors caused by dynamic model errors and uncertain interference is presented. The proposed filtering algorithm is improved by a robust estimation method, wherein both single and constantly changed outliers are considered. In addition, the proposed algorithm was verified by the GPS/INS integrated navigation. The results have shown that the proposed algorithm has better performance than other algorithms.
The conclusions can be summarized as follows: (1) The adaptive Kalman filtering algorithms were developed in order to reduce the positioning errors, and the proper adaptive factors were selected. The H-infinity filtering algorithm performed well in the GPS/INS integrated navigation system that contained the uncertainties. However, the performance was greatly affected by the outliers. (2) The integration of the adaptive Kalman filter, the H-infinity filter, and the robust estimation method provided the AHF algorithm, which can address the deviations caused by dynamic model errors and interference. Since the proposed algorithm was verified by real data, a wide application of the proposed AHF algorithm in dynamic navigation and positioning can be expected.