# A Novel Robust Interval Kalman Filter Algorithm for GPS/INS Integrated Navigation

Authors: Chen Jiang; Shu-bi Zhang; Qiu-zhao Zhang
Journal: Journal of Sensors
 (2016)

---

## Abstract

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).


---

## Body

## 1. Introduction

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 [1]. 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) [2], the Unscented Kalman Filter (UKF) [3, 4], the Particle Filter (PF) [5, 6], the H∞ 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 H∞ filter require relatively more calculations and the latter breaks down in the presence of outliers [14]. The AKF algorithm, which is suitable to balance the dynamic model information and the measurements [15], 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 [18] 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 [19], the interval intersection arithmetic was introduced in an improved IKF algorithm [20]. 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 [27]. 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 Section2, 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 as(1)xk+1=Φkxk+Γkwk,zk=Hkxk+vk,where xk is the system state vector, z is the observation vector, Φk is the state-transition matrix, and wk and vk are the system and observation noises, respectively. Φk,  Γk, and Hk are constant matrices, and k denotes the epoch. The optimal estimate is obtained with the standard KF algorithm when Φk,  Γk, and Hk are fixed. Influences of these uncertain matrices can be controlled by interval matrices:(2)ΦkI=Φk+ΔΦk=Φk-ΔΦk,Φk+ΔΦk,ΓkI=Γk+ΔΓk=Γk-ΔΓk,Γk+ΔΓk,HkI=Hk+ΔHk=Hk-ΔHk,Hk+ΔHk,where ΔΦk,  ΔΓk, and ΔHk are constant perturbation matrices with respective boundaries and the process of the IKF algorithm is stated below [18, 28].Main Process (3) x ^ k I = Φ k - 1 I x ^ k - 1 I + K k I z k I - H k I Φ k - 1 I x ^ k - 1 I .Coprocess (4) P k - 1 I = Φ k - 1 I P k - 1 I Φ k - 1 I T + Γ k - 1 I Q k - 1 Γ k - 1 I T , (5) K k I = P k - 1 I H k I T H k I P k - 1 I H k I T + R k - 1 , (6) P k I = I - K k I H k I P k - 1 I ,where Pk-1I is the a priori covariance matrix, KkI is the gain matrix, x^kI is the estimated state vector of the filter, Qk and Rk are covariance matrices, and PkI is the updated covariance matrix.Three types of arithmetic for the perturbation matrices are adopted in the IKF algorithm: addition, multiplication, and inversion. Assume thatA is an interval matrix, M(A) is the matrix composed of the middle points of each element in A, W(A) is the width of the interval matrix, and the addition arithmetic and multiplication arithmetic are defined as follows:(7)MA1+A2=MA1+MA2,WA1+A2=WA1+WA2,MA1A2=MA1MA2,WA1A2=MA1WA2+WA1MA2,where A1 and A2 are interval matrices. Meanwhile, the inversion arithmetic should be implemented, namely, [HkIPk-1I(HkI)T+Rk]-1 in (5),(8)HkIPk-1IHkIT+Rk-1=HkPk-1HkT+ΔRk-1,ΔRk=HkPk-1ΔHkT+HkΔPk-1HkT+HkΔPk-1ΔHkT+ΔHkPk-1HkT+ΔHkPk-1ΔHkT+ΔHkΔPk-1HkT+ΔHkΔPk-1ΔHkT+Rk.In this IKF algorithm, ΔRk was replaced by the matrix Rk′ which was calculated by all the upper bounds of the interval elements of ΔRk [18]; 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 vectorX^ is listed below:(9)X^=δx,δy,δz,δvx,δvy,δvz,δϕe,δϕn,δϕu,δgx,δgy,δgz,δax,δay,δaz.The state equation of the continuous system is(10)Xt=FtXt+GtWt,where Wt is the system noise and Ft is the dynamic matrix.The discretization of (10) is achieved by Taylor series expansion:(11)Xk=Φk,k-1Xk-1+Wk,where Xk-1 is the state vector in epoch k-1, Wk is the model deviation, and Φk,k-1 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:(12)Zρt=ρGPS-ρINS.The position and velocity differences between GPS and INS are applied to construct the measurement equation. xGPS,  yGPS,  zGPS,  vXGPS,  vYGPS, and vZGPS are the output information of the GPS in the WGS-84 coordinate system, and the output information of the INS is denoted by xINS,  yINS,  zINS, vXINS,  vYINS, and vZINS. The measurement equation is as follows:(13)Lk=rGPS-rINSvGPS-vINS,where Lk is the integrated measurement vector and rGPS,  rINS, vGPS, and  vINS 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 [29].


### 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 [20]. Equations of the reduced IKF algorithm are listed below:(14)x_^k-,x^-k-=Φk,k-1x_^k-1-,x^-k-1-,(15)Pk-=Φk,k-1Pk-1Φk,k-1T+Γk-1Qk-1Γk-1T,(16)Kk=Pk-HkTHkPk-HkT+Rk-1,(17)x_^k,x^-k=I-KkHkx_^k-,x^-k-+Kkzk,(18)Pk=I-KkHkPk-1,where [x_^k-,x^-k-] and [x_^k,x^-k] denote the lower and the upper bounds of the a priori and the a posteriori estimates, respectively, Kk is the gain matrix, and zk is the innovation vector. Equation (16) shows that the covariance matrix of the measurement noise; namely, R, 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:Time Update (19) x _ ^ k - , x ^ - k - = Φ k , k - 1 x _ ^ k - 1 - , x ^ - k - 1 - , P k - = Φ k , k - 1 P k - 1 Φ k , k - 1 T + Γ k - 1 Q k - 1 Γ k - 1 T .Measurement Update (20) K - k = P k - H k T H k P k - H k T + P - k - 1 - 1 , (21) x _ ^ k , x ^ - k = x _ ^ k - , x ^ - k - + K - k L k - H k x _ ^ k - , x ^ - k - , (22) P k = I - K - k H k P k - I - K - k H k T + K - k P - k - 1 K - k T ,where K-k is the equivalent gain matrix, P-k is the equivalent weight matrix of the observation vector and P-k-1=R-k, and R-k 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 [30]. The IGGIII function is adopted to evaluate the equivalent weight matrix P-k. Considering the preceding information, the IGGIII function is defined by [25, 31, 32](23)P-i=pi,v~i≤c0pic0v~ic1-v~ic1-c02,c0<v~i≤c10,v~i>c1,where P-i is the ith diagonal element of the equivalent weight matrix, v~i is the ith standardized residual, and c0 and c1 are the critical values determined by the significance level. In general, c0 is chosen between 1.5 and 2.0 and c1 is chosen between 3.0 and 8.5 [27].The final filtering algorithm applied to loosely coupled GPS/INS integrated navigation is shown in Figure1.Figure 1
A flow chart of the RIKF algorithm for a loosely coupled GPS/INS integrated navigation.


## 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 vectorX^ is listed below:(9)X^=δx,δy,δz,δvx,δvy,δvz,δϕe,δϕn,δϕu,δgx,δgy,δgz,δax,δay,δaz.The state equation of the continuous system is(10)Xt=FtXt+GtWt,where Wt is the system noise and Ft is the dynamic matrix.The discretization of (10) is achieved by Taylor series expansion:(11)Xk=Φk,k-1Xk-1+Wk,where Xk-1 is the state vector in epoch k-1, Wk is the model deviation, and Φk,k-1 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:(12)Zρt=ρGPS-ρINS.The position and velocity differences between GPS and INS are applied to construct the measurement equation. xGPS,  yGPS,  zGPS,  vXGPS,  vYGPS, and vZGPS are the output information of the GPS in the WGS-84 coordinate system, and the output information of the INS is denoted by xINS,  yINS,  zINS, vXINS,  vYINS, and vZINS. The measurement equation is as follows:(13)Lk=rGPS-rINSvGPS-vINS,where Lk is the integrated measurement vector and rGPS,  rINS, vGPS, and  vINS 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 [29].


## 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 [20]. Equations of the reduced IKF algorithm are listed below:(14)x_^k-,x^-k-=Φk,k-1x_^k-1-,x^-k-1-,(15)Pk-=Φk,k-1Pk-1Φk,k-1T+Γk-1Qk-1Γk-1T,(16)Kk=Pk-HkTHkPk-HkT+Rk-1,(17)x_^k,x^-k=I-KkHkx_^k-,x^-k-+Kkzk,(18)Pk=I-KkHkPk-1,where [x_^k-,x^-k-] and [x_^k,x^-k] denote the lower and the upper bounds of the a priori and the a posteriori estimates, respectively, Kk is the gain matrix, and zk is the innovation vector. Equation (16) shows that the covariance matrix of the measurement noise; namely, R, 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:Time Update (19) x _ ^ k - , x ^ - k - = Φ k , k - 1 x _ ^ k - 1 - , x ^ - k - 1 - , P k - = Φ k , k - 1 P k - 1 Φ k , k - 1 T + Γ k - 1 Q k - 1 Γ k - 1 T .Measurement Update (20) K - k = P k - H k T H k P k - H k T + P - k - 1 - 1 , (21) x _ ^ k , x ^ - k = x _ ^ k - , x ^ - k - + K - k L k - H k x _ ^ k - , x ^ - k - , (22) P k = I - K - k H k P k - I - K - k H k T + K - k P - k - 1 K - k T ,where K-k is the equivalent gain matrix, P-k is the equivalent weight matrix of the observation vector and P-k-1=R-k, and R-k 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 [30]. The IGGIII function is adopted to evaluate the equivalent weight matrix P-k. Considering the preceding information, the IGGIII function is defined by [25, 31, 32](23)P-i=pi,v~i≤c0pic0v~ic1-v~ic1-c02,c0<v~i≤c10,v~i>c1,where P-i is the ith diagonal element of the equivalent weight matrix, v~i is the ith standardized residual, and c0 and c1 are the critical values determined by the significance level. In general, c0 is chosen between 1.5 and 2.0 and c1 is chosen between 3.0 and 8.5 [27].The final filtering algorithm applied to loosely coupled GPS/INS integrated navigation is shown in Figure1.Figure 1
A flow chart of the RIKF algorithm for a loosely coupled GPS/INS integrated navigation.


## 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.25m2 and 0.0025 m2/s2, respectively. Errors of the initial position and the initial speed were 1.0 m and 0.1 m/s, 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)Case 1 (initial observations).
Position errors of the four schemes are shown in Figures2, 3, 4, and 5.Figure 2
Position errors of the KF algorithm.Figure 3
Position errors of the IKF algorithm.Figure 4
Position errors of the RKF algorithm.Figure 5
Position errors of the RIKF algorithm.The filtering results were mainly affected by the deviations of the system model because only few outlying observations exist. It can be seen from Figures2 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 Table1.Table 1
RMSEs of each algorithm (m).
Axis KF IKF RKF RIKF X 0.130 0.092 0.093 0.090 Y 0.230 0.180 0.207 0.171 Z 0.118 0.102 0.109 0.099Table1 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 Figures6, 7, 8, and 9.Figure 6
Position errors of the KF algorithm.Figure 7
Position errors of the IKF algorithm.Figure 8
Position errors of the RKF algorithm.Figure 9
Position errors of the RIKF algorithm.Figure6 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 Table2.Table 2
RMSE of each algorithm (m).
Axis KF IKF RKF RIKF X 0.454 0.286 0.106 0.099 Y 0.538 0.365 0.215 0.179 Z 0.436 0.292 0.123 0.117The 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.


## 5. Conclusions

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.



---

*Source: 3727241-2016-10-13.xml*