Research Article | Open Access
Chen Jiang, Shu-bi Zhang, Qiu-zhao Zhang, "A Novel Robust Interval Kalman Filter Algorithm for GPS/INS Integrated Navigation", Journal of Sensors, vol. 2016, Article ID 3727241, 7 pages, 2016. https://doi.org/10.1155/2016/3727241
A Novel Robust Interval Kalman Filter Algorithm for GPS/INS Integrated Navigation
Kalman filter is widely applied in data fusion of dynamic systems under the assumption that the system and measurement noises are Gaussian distributed. In literature, the interval Kalman filter was proposed aiming at controlling the influences of the system model uncertainties. The robust Kalman filter has also been proposed to control the effects of outliers. In this paper, a new interval Kalman filter algorithm is proposed by integrating the robust estimation and the interval Kalman filter in which the system noise and the observation noise terms are considered simultaneously. The noise data reduction and the robust estimation methods are both introduced into the proposed interval Kalman filter algorithm. The new algorithm is equal to the standard Kalman filter in terms of computation, but superior for managing with outliers. The advantage of the proposed algorithm is demonstrated experimentally using the integrated navigation of Global Positioning System (GPS) and the Inertial Navigation System (INS).
The integration of GPS and INS has been widely adopted for dynamic navigation and positioning. The Kalman filter (KF) is the most notable real-time optimal estimator, which has found an extremely broad field of application . KF has tacitly become an indispensable data fusion approach for GPS/INS integrated navigation. However, the standard KF algorithm can hardly deal with nonlinear and robust problems. Numerous nonlinear filtering and robust algorithms were developed, such as the Extended Kalman Filter (EKF) , the Unscented Kalman Filter (UKF) [3, 4], the Particle Filter (PF) [5, 6], the filter [7, 8], the Adaptive Kalman filter (AKF) [9–12], and the robust Kalman filter (RKF) [13, 14]. A nonlinear system can be handled by EKF, but the impacts of higher order terms are neglected. The advantages manifested by UKF lie in the concise computation and real-time processing. Nevertheless, it becomes unstable in a high-dimensional system. Both the PF and the filter require relatively more calculations and the latter breaks down in the presence of outliers . The AKF algorithm, which is suitable to balance the dynamic model information and the measurements , has been widely investigated; however, some of the AKF algorithms cannot control the effects of outliers. Closely related to the model uncertainty, the RKF exerts a tremendous fascination on researchers, whereas it ignores the system model noise. Yang et al. proposed the adaptively robust filter algorithm [15–17] which manifested strong stability and flexibility, and the adaptively robust filter shows better ability to control the influences of both the dynamic model disturbances and the outlying measurements.
In the standard KF algorithm, the system model and statistical properties of the Gaussian white noise must be known in order to achieve optimal results. However, the actual system and noise models differ invariably from the assumptions. A novel algorithm named the interval Kalman filter (IKF) was proposed  to cope with model parameter uncertainties. The effects of the model errors are weakened using the IKF algorithm, and the optimality of the KF approach is preserved. In addition, the IKF algorithm is performed with reasonable amount of computation. On the basis of interval addition, multiplication, and inversion arithmetic , the interval intersection arithmetic was introduced in an improved IKF algorithm . To simplify the calculation process, model errors were assumed to be included only in the a priori and a posteriori estimates. Nevertheless, covariance matrices of the system noise and observation noise were considered to be known and fixed. The KF approach is not the optimal one once the observations become contaminated. Consequently, the influences of outliers must be considered. The robust estimation method provides a way to control the influences of outliers [21, 22]. In the field of robust estimation, a series of equivalent weight functions is constructed [23–26] to resist the impacts of outliers. Among these equivalent weight functions, the concept of the IGG procedure is more suitable for the geodetic surveying errors. Rather than disproportionately pursuing validity and unbiasedness, the robust estimation method focuses on the robustness and the reliability of the estimator . Accordingly, it is desirable to develop a new filtering scheme by combining the robust estimation method with the IKF.
This paper focuses on a comprehensive filtering algorithm using the reduced IKF and the classic equivalent weight robust estimation method for the GPS/INS integrated navigation. With the impacts of the abnormal observations and the model errors weakened, a novel filtering algorithm is developed for the GPS/INS integrated navigation. Practical experiments of GPS/INS integrated dynamic navigation are implemented. Finally, the performance is evaluated and the suitability of the proposed filtering algorithm is discussed.
The rest of the paper is organized as follows. In Section 2, the theory of the IKF is introduced and the model deviations of the convention KF algorithm are analyzed. In Section 3, the robust interval Kalman filter (RIKF) algorithm is proposed based on the robust estimation and the IKF. Section 4 presents the advantages of the new algorithm which are verified by experiments, and a comparison with other different algorithms is performed and discussed. The conclusions are presented in Section 5.
2. The Interval Kalman Filter (IKF)
It is extremely difficult to establish a physical model and noise model of the irregularly motional carrier. Therefore, fixing dynamic system parameters and noise statistical parameters becomes a crucial assumption when the standard KF algorithm is applied. However, in most cases, the system and observation noise terms are only approximately known. Thus, the IKF algorithm was proposed to process the dynamic system with uncertainties in the model parameters.
The system equation and observation equation are expressed aswhere is the system state vector, is the observation vector, is the state-transition matrix, and and are the system and observation noises, respectively. , , and are constant matrices, and denotes the epoch. The optimal estimate is obtained with the standard KF algorithm when , , and are fixed. Influences of these uncertain matrices can be controlled by interval matrices:where , , and are constant perturbation matrices with respective boundaries and the process of the IKF algorithm is stated below [18, 28].
Coprocesswhere is the a priori covariance matrix, is the gain matrix, is the estimated state vector of the filter, and are covariance matrices, and is the updated covariance matrix.
Three types of arithmetic for the perturbation matrices are adopted in the IKF algorithm: addition, multiplication, and inversion. Assume that is an interval matrix, is the matrix composed of the middle points of each element in , is the width of the interval matrix, and the addition arithmetic and multiplication arithmetic are defined as follows:where and are interval matrices. Meanwhile, the inversion arithmetic should be implemented, namely, in (5),In this IKF algorithm, was replaced by the matrix which was calculated by all the upper bounds of the interval elements of ; thus, the matrix inversion arithmetic becomes much easier. Similar to the standard KF, the IKF algorithm is composed of a homogeneous recursive structure.
3. The GPS/INS Integrated Navigation and the Robust Interval Kalman Filter (RIKF)
3.1. GPS/INS Integrated Navigation
In loosely coupled GPS/INS integrated navigation, the differences between the outputs (position and velocity) of GPS and INS are regarded as the observation information. A 15-dimension state vector is selected which includes the deviations of the position, the deviations of velocities, the attitudes, and the noises of the gyroscope and the accelerometer. The state vector is listed below:The state equation of the continuous system iswhere is the system noise and is the dynamic matrix.
The discretization of (10) is achieved by Taylor series expansion:where is the state vector in epoch , is the model deviation, and is the discretized state-transition matrix.
There are two kinds of integration for integrated navigation, namely, the tightly coupled and the loosely coupled. In the tightly coupled navigation, the pseudorange difference between GPS and INS is considered as the measurement equation:The position and velocity differences between GPS and INS are applied to construct the measurement equation. , , , , , and are the output information of the GPS in the WGS-84 coordinate system, and the output information of the INS is denoted by , , , , , and . The measurement equation is as follows:where is the integrated measurement vector and , , , and are the position and the velocity information output of GPS and INS, respectively. Compared to the loosely coupled integration, the tightly coupled integration performs better in terms of precision. On the other hand, the loosely coupled integration is easier to be implemented and the computation process is more concise .
3.2. The Robust Interval Kalman Filter
After the addition, multiplication, and inversion calculations, the estimation results of the interval matrices are similar to that of the central point of the interval matrix, and the difference lies in the interval width. Furthermore, the interval width may become infinite or zero in some cases. Thus, the IKF and the standard KF differ little in process.
Intersection arithmetic of the interval matrix is introduced, and model errors are reduced into the a priori and a posteriori estimates. Hence, required calculations can be reduced, and the computational speed of the reduced IKF algorithm becomes almost identical to the standard KF algorithm . Equations of the reduced IKF algorithm are listed below:where and denote the lower and the upper bounds of the a priori and the a posteriori estimates, respectively, is the gain matrix, and is the innovation vector. Equation (16) shows that the covariance matrix of the measurement noise; namely, , is regarded as a constant and fixed matrix. This may result in divergence of the filtering algorithm due to outliers in the observations.
The robust estimation method is introduced into the reduced IKF algorithm, and the equations of the RIKF algorithm are as follows:
Measurement Updatewhere is the equivalent gain matrix, is the equivalent weight matrix of the observation vector and , and is the equivalent covariance matrix. Equation (22) named the Joseph stabilized version is chosen here because it is more numerically stable and robust than other forms of the covariance update equation . The IGGIII function is adopted to evaluate the equivalent weight matrix . Considering the preceding information, the IGGIII function is defined by [25, 31, 32]where is the th diagonal element of the equivalent weight matrix, is the th standardized residual, and and are the critical values determined by the significance level. In general, is chosen between 1.5 and 2.0 and is chosen between 3.0 and 8.5 .
The final filtering algorithm applied to loosely coupled GPS/INS integrated navigation is shown in Figure 1.
4. Practical Experiments of the GPS/INS Integrated Navigation
A vehicle mounted GPS/INS integrated navigation system which is composed of two GPS receivers and an inertial measurement unit (IMU) is implemented. One of the GPS receivers was used as a base station, and another receiver as well as the IMU is mounted on the vehicle.
The loosely coupled mode was used in this experiment and the position and the speed were treated as external observations. They were calculated by the double difference pseudorange of the GPS with variances of 0.25 and 0.0025 , respectively. Errors of the initial position and the initial speed were 1.0 and 0.1 , respectively. The precise results given by the double difference carrier phase were considered as references.
Four schemes are implemented in the integrated navigation data processing unit. These four schemes are listed below: Scheme 1: Kalman filter (KF) Scheme 2: interval Kalman filter (IKF) Scheme 3: robust Kalman filter (RKF) Scheme 4: robust reduced interval kalman filter (RIKF)
The filtering results were mainly affected by the deviations of the system model because only few outlying observations exist. It can be seen from Figures 2 and 3 that the IKF algorithm satisfactorily performed in resisting the deviations of the system model, and the RIKF algorithm manifested a better performance than that of the IKF algorithm in resisting the outliers shown in Figure 5. Comparing Figures 3, 4, and 5, the RKF algorithm is inferior to IKF and RIKF algorithms in handling deviations of the system model.
Statistical results of the Root Mean Squares Errors (RMSE) are listed in Table 1.
Table 1 shows that there was only a small difference between the solutions of these algorithms in terms of RMSE. However, by integrating the advantages of the IKF and the RKF algorithms, the RIKF algorithm achieved a slightly better performance than the other algorithms.
Case 2 (observations with perturbation). Based on the initial observations, gross errors were added to the GPS observations at the 60th, 160th, 260th, 360th, and 460th epochs, respectively. Four filtering algorithms were implemented to examine their performance. Position errors of these four schemes are shown in Figures 6, 7, 8, and 9.
Figure 6 shows that the KF algorithm had a lesser capacity to resist outliers. System stability and perturbation influences on the model were improved by the IKF algorithm. Nevertheless, the filtering results were still greatly affected by the outliers, as concluded from Figure 7. The RKF algorithm was performed using the IGGIII scheme. Figure 8 showed that the perturbation of the model changed little with the RKF algorithm, although the influences of the outliers were decreased significantly. Figure 9 indicated that the impacts of the system model and the perturbation of the noise model and the outliers were significantly weakened with the RIKF algorithm. Moreover, the filtering process became more stable.
Statistical results of the RMSEs are listed in Table 2.
The RIKF algorithm had better fault tolerance and robustness among all other algorithms. The RMSEs of the RIKF algorithm were the smallest over all the coordinate components which signified that the carrier positions calculated by the RIKF algorithm were in better agreement with the reality. In addition, filtering results of the IKF and RKF algorithms are better than that of the KF algorithm. Whereas the RKF algorithm performed even better than the IKF algorithm with respect to the outliers, which was the main goal of the integrated navigation filtering in this experiment.
This paper presented an improved interval Kalman filter algorithm. The recent IKF algorithms focused on controlling the effects of the model errors. However, deviations of the filtering results would appear with the inclusion of outlying observations. This work proposed a robust interval Kalman filter (RIKF) algorithm based on the equivalent weight robust estimation and the interval Kalman filter to compensate for the deviations of the models and the outlying observations simultaneously. The proposed algorithm was validated by implementing data processing of a GPS/INS integrated navigation. A better performance was achieved with the proposed RIKF algorithm compared to previous methods. Overall, the following can be concluded:(1)The existing IKF algorithms performed well with the GPS/INS integrated navigation system which contained uncertainties. However, the performance would be greatly affected by the outlying observations.(2)The RKF algorithm showed great robustness if the measurements contained outliers. Nevertheless, the noise term of the model generally remained unchanged with the RKF algorithm.(3)Comparison of the IKF, the RKF, and the RIKF algorithms with and without outlying observations showed that, by integrating the advantages of the RKF and the IKF algorithms, the proposed RIKF algorithm could resist the deviations of the model and the outlying observations simultaneously. There is broad application for the proposed RIKF algorithm in the dynamic navigation and positioning.
The authors declare that they have no competing interests.
The authors are grateful for the support of the Fundamental Research Funds for the Central Universities (China University of Mining and Technology, no. 2015QNB08) and the Priority Academic Program Development of Jiangsu Higher Education Institutions (PAPD SA1102).
- R. E. Kalman, “A new approach to linear filtering and prediction problems,” Journal of Basic Engineering, vol. 82, no. 1, pp. 35–45, 1960.
- C. Shen, Z. Bai, H. Cao et al., “Optical flow sensor/INS/magnetometer integrated navigation system for MAV in GPS-denied environment,” Journal of Sensors, vol. 2016, Article ID 6105803, 10 pages, 2016.
- E. A. Wan and R. van der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proceedings of the IEEE Adaptive Systems for Signal Processing, Communications and Control Symposium (AS-SPCC '00), pp. 153–158, 2000.
- R. Kandepu, B. Foss, and L. Imsland, “Applying the unscented Kalman filter for nonlinear state estimation,” Journal of Process Control, vol. 18, no. 7-8, pp. 753–768, 2008.
- F. Gustafsson, “Particle filter theory and practice with positioning applications,” IEEE Aerospace and Electronic Systems Magazine, vol. 25, no. 7, pp. 53–81, 2010.
- O. Heirich, “Bayesian train localization with particle filter, loosely coupled GNSS, IMU, and a track map,” Journal of Sensors, vol. 2016, Article ID 2672640, 15 pages, 2016.
- K. M. Nagpal and P. P. Khargonekar, “Filtering and smoothing in an setting,” IEEE Transactions on Automatic Control, vol. 36, no. 2, pp. 152–166, 1991.
- C. E. de Souza, R. M. Palhares, and P. L. D. Peres, “Robust filter design for uncertain linear systems with multiple time-varying state delays,” IEEE Transactions on Signal Processing, vol. 49, no. 3, pp. 569–576, 2001.
- A. H. Mohamed and K. P. Schwarz, “Adaptive Kalman filtering for INS/GPS,” Journal of Geodesy, vol. 73, no. 4, pp. 193–203, 1999.
- W. Ding, J. Wang, C. Rizos, and D. Kinlyside, “Improving adaptive kalman estimation in GPS/INS integration,” Journal of Navigation, vol. 60, no. 3, pp. 517–529, 2007.
- Y. Yang and T. Xu, “An adaptive Kalman filter based on sage windowing weights and variance components,” Journal of Navigation, vol. 56, no. 2, pp. 231–240, 2003.
- Y. Yang and W. Gao, “An optimal adaptive Kalman filter,” Journal of Geodesy, vol. 80, no. 4, pp. 177–183, 2006.
- K. R. Koch and Y. Yang, “Robust Kalman filter for rank deficient observation models,” Journal of Geodesy, vol. 72, no. 7-8, pp. 436–441, 1998.
- M. A. Gandhi and L. Mili, “Robust Kalman filter based on a generalized maximum-likelihood-type estimator,” IEEE Transactions on Signal Processing, vol. 58, no. 5, pp. 2509–2520, 2010.
- Y. Yang, H. He, and G. Xu, “Adaptively robust filtering for kinematic geodetic positioning,” Journal of Geodesy, vol. 75, no. 2-3, pp. 109–116, 2001.
- Y. Yang and W. Gao, “Comparison of two fading filters and adaptively robust filter,” Geo-Spatial Information Science, vol. 10, no. 3, pp. 200–203, 2007.
- Y. Yang and X. Cui, “Adaptively robust filter with multi adaptive factors,” Survey Review, vol. 40, no. 309, pp. 260–270, 2008.
- G. Chen, J. Wang, and L. S. Shieh, “Interval Kaiman filtering,” IEEE Transactions on Aerospace and Electronic Systems, vol. 33, no. 1, pp. 250–259, 1997.
- G.-L. Du, P. Zhang, and X. Liu, “Markerless humanmanipulator interface using leap motion with interval Kalman filter and improved particle filter,” IEEE Transactions on Industrial Informatics, vol. 12, no. 2, pp. 694–704, 2016.
- Y. Shen, L. Zhang, Z.-Q. Fu, and J.-Y. Wang, “Interval Kalman filtering algorithm for high dynamic navigation and positioning,” Journal of Astronautics, vol. 34, no. 3, pp. 355–361, 2013.
- J. Zhou, “Classical theory of errors and robust estimation,” Acta Geodaetica et Cartographica Sinica, vol. 18, no. 2, pp. 115–120, 1989.
- Y. Yang, “Robust estimation of geodetic datum transformation,” Journal of Geodesy, vol. 73, no. 5, pp. 268–274, 1999.
- P. J. Huber, “Robust estimation of a location parameter,” The Annals of Mathematical Statistics, vol. 35, no. 2, pp. 73–101, 1964.
- F. R. Hampel, E. M. Ronchetti, P. J. Rousseeuw, and W. A. Stahel, Robust Statistics: The Approach Based on Influence Functions, vol. 114, John Wiley & Sons, New York, NY, USA, 2011.
- Y. Yang, “Robust estimation for dependent observations,” Manuscripta Geodeatica, vol. 19, no. 1, pp. 10–17, 1994.
- Y.-X. Yang and F.-M. Wu, “Modified equivalent weight function with variable criterion for robust estimation,” Journal of Zhengzhou Institute of Surveying and Mapping, vol. 23, no. 5, pp. 317–320, 2006.
- Y.-X. Yang, Adaptive Navigation and Dynamic Positioning, Surveying and Mapping Press, 2006.
- X.-F. He and G. Yang, “Extended interval Kalman filter and its application in nonlinear GPS/INS integrated system,” Acta Geodaetica et Cartographica Sinica, vol. 33, no. 1, pp. 47–52, 2004.
- W.-G. Gao, Y.-X. Yang, and S.-C. Zhang, “Adaptive robust Kalman filtering based on the current statistical model,” Acta Geodaetica et Cartographica Sinica, vol. 35, no. 1, pp. 15–18, 2006.
- G. Chang, “Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion,” Journal of Geodesy, vol. 88, no. 4, pp. 391–401, 2014.
- Y. Yang, L. Song, and T. Xu, “Robust estimator for correlated observations based on bifactor equivalent weights,” Journal of Geodesy, vol. 76, no. 6, pp. 353–358, 2002.
- Y. Yang and J. Xu, “GNSS receiver autonomous integrity monitoring (RAIM) algorithm based on robust estimation,” Geodesy and Geodynamics, vol. 7, no. 2, pp. 117–123, 2016.
Copyright © 2016 Chen Jiang et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.