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

Abstract

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.

1. 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 eliminate the effects of outliers [8]. In the DIA methods, the outliers are detected and identified firstly, then deviations of the state estimates caused by the outliers are eliminated, finally, the adaptation is implemented in which the model is recovered from the identified errors. However, the identification is quite difficult, especially when the measurements are not accurate [9]. For instance, the detection and identification process are difficult to implement when the measurements contain multiple outliers or the continuously-changing outliers. Therefore, in order to weaken the influence of the outliers, it is essential to research the filtering and estimation algorithms which are more robust in the GPS/INS integrated navigation system [9,10,11].

To achieve a better performance with the GPS/INS integrated navigation system, the modeling and estimation problem should be addressed properly [7]. The former is concerned with developing better error models and the latter requires proper use of the estimation algorithm and the measurement information. In the Kalman filter, filter divergence often happens when big errors of the system model exist, due to the effects of the improper a prior information [12]. With a constant fading factor, the fading Kalman filter was proposed to overcome the shortcomings of the Kalman filter [13]. In recent years, research about the fading Kalman filter focuses on the estimation of the fading factor. An optimal adaptive algorithm with a single fading factor for fading Kalman filter was proposed [14], nevertheless, a single fading factor can hardly guarantee the optimization of the whole system model, and the filter divergence will be likely to happen when handling complex system model [15]. Various adaptive Kalman filters have been developed to achieve a better estimator [9,16,17,18]. Then the multiple fading filter [15,19] was proposed to improve the performance of the fading filter. In the field of navigation and positioning, an algorithm based on the magnitude of the predicted residuals was proposed to reduce the dynamic model errors [20]. However, most of the above algorithms show few abilities to control the influences of the uncertain interferences. Moreover, the GPS/INS integrated navigation is very likely to be affected by uncertainties due to the exogenous disturbances, device damage, and inaccurate sensor noise statistical information [10]. By minimizing the estimation error in the worst case, the H-infinity filtering algorithm provides an effective way to improve the robustness of the system model [21] and it can be used to address the uncertainties in the measurement noise [18,22].

This paper focuses on a comprehensive filtering algorithm for the GPS/INS integrated navigation system by combining the advantages of the multiple fading filter and the H-infinity filter. Based on the concept of fading memory and the optimization of the filter, a new multiple fading factor is proposed. Experiments are implemented with the data collected under actual conditions. Performances of different filtering algorithms are evaluated and the contrastive analysis are implemented. Results demonstrate that the new multiple fading filter algorithm shows a higher accuracy whether the abnormal measurements ever exist. In addition, the proposed fading filtering algorithm is easy to implement with a relatively reasonable computation burden.

The rest of this paper is organized as follows: in Section 2, the theory of the fading filter and the H-infinity filter are introduced, and the nonlinear form of the H-infinity filter is listed. In Section 3, a new multiple fading factor is proposed and a comprehensive filtering algorithm is constructed. In Section 4, rules of the GPS/INS integrated navigation system are introduced, and equations of the system for the loosely-coupled integrated navigation system are listed. Section 5 presents the advantages of the proposed filtering algorithm through actual experiments, and the results are discussed and analyzed. The conclusions are provided in Section 6.

2. Fading Filter and H-Infinity Filter

In this section, the basic rules of the fading filter are introduced and the recent fading factors are discussed, then the principles of the H-infinity filter and its nonlinear form are discussed.

2.1. Basic Rules of the Fading Filter

For the dynamic model equation and the measurement equation:

$$\{\begin{array}{l}{x}_k={\mathsf{\Phi }}_{k,k-1}{x}_{k-1}+w_k\\ z_k=H_k{x}_k+v_k\end{array},$$

(1)

the predicted state vector is presented as:

$$x_{k/k-1}={\mathsf{\Phi }}_{k,k-1}{x}_{k-1},$$

(2)

where $x_k$ denotes the state vector at the epoch $k$, ${\mathsf{\Phi }}_{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:

$$x_{k/k}=x_{k/k-1}+{\overline{K}}_k(z_k-H_k{x}_{k/k-1}),$$

(3)

$${\overline{K}}_k={\overline{P}}_{k/k-1}{H}_k^{\mathrm{T}}{(H_k{\overline{P}}_{k/k-1}{H}_k^{\mathrm{T}}+R_k)}^{-1},$$

(4)

$${\overline{P}}_{k/k-1}=S_k{\mathsf{\Phi }}_{k,k-1}{P}_{k-1}{{\mathsf{\Phi }}^{\mathrm{T}}}_{k,k-1}+Q_k,$$

(5)

where ${\overline{K}}_k$ denotes the equivalent gain matrix, ${\overline{P}}_{k/k-1}$ denotes the equivalent covariance matrix of the state vector, $S_k$ denotes the fading factor and $S_k\ge 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:

$$P_{k/k-1}={\mathsf{\Phi }}_{k,k-1}{P}_{k-1}{{\mathsf{\Phi }}^{\mathrm{T}}}_{k,k-1}+Q_k.$$

(6)

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.

2.2. 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:

$$S_k=\max \{1,\frac{1}{n}tr(N_k{M}_k^{-1})\},$$

(7)

where:

$$M_k=H_k{\mathsf{\Phi }}_{k,k-1}{P}_{k-1}{\mathsf{\Phi }}_{k,k-1}^{\mathrm{T}}{H}_k^{\mathrm{T}},$$

(8)

$$N_k=P_{V_k}-H_k{Q}_k{H}_k^{\mathrm{T}}-R_k,$$

(9)

$$P_{V_k}=E(V_k{V}_k^{\mathrm{T}}),$$

(10)

$$V_k=H_k{x}_{k/k-1}-z_k,$$

(11)

where $tr(\cdot )$ denotes taking trace of a matrix, $V_k$ denotes the predicted residual vector, and $P_{V_k}$ denotes the covariance matrix of $V_k$, and ${\widehat{P}}_{V_k}=\frac{1}{k}{\displaystyle \sum _{i=1}^k{V}_i}{V}_i^{\mathrm{T}}$ [12]. Then Xia [14] proposed a simplified method, namely:

$$S_k=\max \{1,tr(N_k)/tr(M_k)\},$$

(12)

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_k=\mathrm{diag}(s_1,s_2,...,s_t,1,...,1,...,1),$$

(13)

$$s_i=\max \left(1,\sqrt{\frac{{[v_i(k)]}^2}{{\lambda }_i^2{j}_{ii}(k){\epsilon }_i}-\frac{b_{ii}(k)}{j_{ii}(k)}}\right),(i=1,2,...,t),$$

(14)

where $v_i(k)$ is the $i-\mathrm{th}$ element of $V_k$, $t$ is the dimension of the measurement equation, ${\lambda }_i$ is the $i-\mathrm{th}$ observable element of $H_k$, $b_{ii}(k)$ is the $i-\mathrm{th}$ diagonal element of the matrix $B_k$ and $B_k=H_k{Q}_{k-1}{H}_k^{\mathrm{T}}+R_k$, $j_{ii}(k)$ is the $i-\mathrm{th}$ diagonal element of the matrix $J_k$ and $J_k={\mathsf{\Phi }}_k{P}_{k-1}{\mathsf{\Phi }}_k^{\mathrm{T}}$, ${\epsilon }_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.

2.3. Principles of the H-Infinity Filter

Assume that the dynamic model equation and the measurement equation is given by:

$$\{\begin{array}{l}{x}_k={\mathsf{\Phi }}_{k,k-1}{x}_{k-1}+w_k\\ y_k=L_k{x}_k\\ z_k=H_k{x}_k+v_k\end{array},$$

(15)

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]:

$$J=\frac{{\displaystyle \sum _{k=1}^N{\Vert x_k-{\widehat{x}}_k\Vert }^2}}{{\Vert x_0-{\widehat{x}}_0\Vert }_{P_0^{-1}}^2+{\displaystyle \sum _{k=1}^N({\Vert w_k\Vert }_{Q_k^{-1}}^2+{\Vert v_k\Vert }_{R_k^{-1}}^2)}},$$

(16)

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$, ${\widehat{x}}_0$ and ${\widehat{x}}_k$ denote the estimated state vectors of $x_0$ and $x_k$, respectively, and the expression ${\Vert x_0-{\widehat{x}}_0\Vert }_{P_0^{-1}}^2$ denotes ${(x_0-{\widehat{x}}_0)}^{\mathrm{T}}{P}_0^{-1}(x_0-{\widehat{x}}_0)$.

The estimate of the state vector should be achieved under the conditions of ${\widehat{x}}_k=\arg \min {\Vert J\Vert }_{\infty }$. 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 $\gamma$ which satisfied the following Riccati inequality [1]:

$$P_k^{-1}+H_k^{\mathrm{T}}{H}_k-{\gamma }^2{L}_k^{\mathrm{T}}{L}_k>0,$$

(17)

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]:

$$P_{k/k}=P_{k/k-1}-{\mathsf{\Phi }}_{k/k-1}{P}_{k/k-1}[H_k^{\mathrm{T}}{L}_k^{\mathrm{T}}]R_{e,k}^{-1}\left[\begin{array}{l}{H}_k\\ L_k\end{array}\right]P_{k/k-1}{\mathsf{\Phi }}_{k/k-1}^{\mathrm{T}},$$

(18)

$$R_{e,k}^{-1}=\left[\begin{array}{cc}I\hfill & \hfill 0\\ 0\hfill & \hfill -{\gamma }^{2}I\end{array}\right]+\left[\begin{array}{l}{H}_k\\ L_k\end{array}\right]P_{k/k-1}[H_k^{\mathrm{T}}{L}_k^{\mathrm{T}}].$$

(19)

It should be noticed that the robustness of the filter is related to the threshold value $\gamma$, and the filter becomes more robust when $\gamma$ decreases. However, it may cause a filter divergence if $\gamma$ is too close to zero [23].

3. 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:

$$P_{k/k-1}=S_k{\mathsf{\Phi }}_{k,k-1}{P}_{k-1}{{\mathsf{\Phi }}^{\mathrm{T}}}_{k,k-1}+Q_k,$$

(20)

$S_k=\mathrm{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:

$$P_{k/k-1}=S_k{\mathsf{\Phi }}_{k,k-1}{P}_{k-1}{{\mathsf{\Phi }}^{\mathrm{T}}}_{k,k-1}{S}_k^{\mathrm{T}}+Q_k.$$

(21)

Based on the observability and the optimal fading factor, a new multiple fading factor is constructed as:

$$s_i=\{\begin{array}{ll}\max \{1,[N_k(i,i)]/[M_k(i,i)]\},& i=1,2,...,t\\ 1& ,\mathrm{other}\end{array},$$

(22)

where $N_k(i,i)$ and $M_k(i,i)$ represent the $i-\mathrm{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)):

$${\overline{P}}_{k/k-1}=S_k{\mathsf{\Phi }}_{k,k-1}{P}_{k-1/k-1}{\mathsf{\Phi }}_{k,k-1}^{\mathrm{T}}{S}_k^{\mathrm{T}}+Q_k,$$

(23)

$$x_{k/k}=x_{k/k-1}+{\overline{K}}_k(z_k-H_k{x}_{k/k-1}),$$

(24)

$${\overline{K}}_k={\overline{P}}_{k/k-1}{H}_k^{\mathrm{T}}{(H_k{\overline{P}}_{k/k-1}{H}_k^{\mathrm{T}}+R_k)}^{-1},$$

(25)

$${\overline{P}}_{k/k}={\overline{P}}_{k/k-1}-{\mathsf{\Phi }}_{k/k-1}{\overline{P}}_{k/k-1}[H_k^{\mathrm{T}}{L}_k^{\mathrm{T}}]{\overline{R}}_{e,k}^{-1}\left[\begin{array}{l}{H}_k\\ L_k\end{array}\right]{\overline{P}}_{k/k-1}{\mathsf{\Phi }}_{k/k-1}^{\mathrm{T}},$$

(26)

$${\overline{R}}_{e,k}^{-1}=\left[\begin{array}{cc}I\hfill & \hfill 0\\ 0\hfill & \hfill -{\gamma }^{2}I\end{array}\right]+\left[\begin{array}{l}{H}_k\\ L_k\end{array}\right]{\overline{P}}_{k/k-1}[H_k^{\mathrm{T}}{L}_k^{\mathrm{T}}],$$

(27)

where ${\overline{P}}_{k/k}$ denotes the a posteriori covariance matrix of the state, and $S_k$ is fixed through (22).

4. 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 vector $\widehat{X}$ is given by:

$$\widehat{X}=[\delta x,\delta y,\delta z,\delta v_x,\delta v_y,\delta v_z,\delta {\phi }_e,\delta {\phi }_n,\delta {\phi }_u,\delta g_x,\delta g_y,\delta g_z,\delta a_x,\delta a_y,\delta a_z].$$

(28)

The nonlinear differential error model for the INS system is defined by [6,27]:

$$\{\begin{array}{l}\Delta {\dot{R}}^e=\Delta V^e\\ \Delta {\dot{V}}^e=(I_{3\times 3}-C_{e^{\prime }}^e)f^{e^{\prime }}+C_b^{e^{\prime }}{\nabla }^b-2{\mathsf{\Omega }}_{ie}^e\Delta V^e\\ {\dot{\phi }}^e=(I_{3\times 3}-C_e^{e^{\prime }}){\omega }_{ie}^e-C_b^{e^{\prime }}{\epsilon }^b\\ {\dot{\nabla }}^b=0\\ {\dot{\epsilon }}^b=0\end{array},$$

(29)

where $\Delta R^e$ and $\Delta V^e$ are the position and velocity errors under the computer $e$ frame, respectively, ${\dot{\phi }}^e$ is the attitude error between $e$ frame and the platform $e^{\prime }$ frame, $I_{3\times 3}$ is the identity matrix, $C_{e^{\prime }}^e$ is the rotation matrix between $e$ and $e^{\prime }$ frame, $C_b^{e^{\prime }}$ is the rotation matrix between the body frame and $e^{\prime }$ frame, ${\mathsf{\Omega }}_{ie}^e$ is the skew symmetric matrix of earth rotation rate ${\omega }_{ie}^e$, ${\nabla }^b$ and ${\epsilon }^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:

$$\{\begin{array}{l}{x}_k=f(x_{k-1})+w_k\\ z_k=h(x_k)+v_k\end{array},$$

(30)

where $f(\cdot )$ and $h(\cdot )$ denote the known nonlinear functions. The cubature points are generated by:

$$\xi =\sqrt{\frac{m}{2}}{\left[1\right]}_i,i=1,...,m,$$

(31)

where $m$ denotes the number of the cubature points and $m=2n$, $n$ denotes the dimension of the state vector, $\left[1\right]$ denotes the integral and symmetrical set of points. Then equations of the cubature Kalman filter are given by [28].

(i) Time update

$$\{\begin{array}{l}{s}_{k-1/k-1}=SVD(P_{k-1/k-1})\\ X_{k-1,k-1}=s_{k-1/k-1}\xi +x_{k-1/k-1}\\ X_{k/k-1}^{\ast }=f(X_{k-1,k-1})\end{array},$$

(32)

$$x_{k/k-1}=\frac{1}{m}{\displaystyle \sum _{i=1}^m{X}_{i,k/k-1}^{\ast }},$$

(33)

$$P_{k/k-1}=\frac{1}{m}{\displaystyle \sum _{i=1}^m{X}_{i,k/k-1}^{\ast }{X}_{i,k/k-1}^{\ast \mathrm{T}}}-x_{k/k-1}{x}_{k/k-1}^{\mathrm{T}}+Q_k.$$

(34)

(ii) Measurement update

$$\{\begin{array}{l}{s}_{k/k-1}=SVD(P_{k/k-1})\\ X_{k/k-1}=s_{k/k-1}\xi +x_{k/k-1}\\ Z_{k/k-1}=h(X_{i,k/k-1})\\ z_{k/k-1}=\frac{1}{m}{\displaystyle \sum _{i=1}^m{Z}_{i,k/k-1}}\\ P_{zz,k/k-1}=\frac{1}{m}{\displaystyle \sum _{i=1}^m{Z}_{i,k/k-1}}{Z}_{i,k/k-1}^{\mathrm{T}}-z_{k/k-1}{z}_{k/k-1}^{\mathrm{T}}+R_k\\ P_{xz,k/k-1}=\frac{1}{m}{\displaystyle \sum _{i=1}^m{X}_{i,k/k-1}}{Z}_{i,k/k-1}^{\mathrm{T}}-x_{k/k-1}{z}_{k/k-1}^{\mathrm{T}}\end{array},$$

(35)

then the final measurement update equations are obtained:

$$K_k=P_{xz,k/k-1}{P}_{zz,k/k-1}^{-1},$$

(36)

$$x_{k/k}=x_{k/k-1}+K_k(z_k-z_{k/k-1}),$$

(37)

$$P_{k/k}=P_{k/k-1}-K_k{P}_{zz,k/k-1}{K}_k^{\mathrm{T}},$$

(38)

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}^{\ast }$ 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:

(i) Time update

$$\{\begin{array}{l}{x}_{k/k-1}=\frac{1}{m}{\displaystyle \sum _{i=1}^m{X}_{i,k/k-1}^{\ast }}\\ P_{k/k-1}=S_k\left(\frac{1}{m}{\displaystyle \sum _{i=1}^m{X}_{i,k/k-1}^{\ast }{X}_{i,k/k-1}^{\ast \mathrm{T}}}-x_{k/k-1}{x}_{k/k-1}^{\mathrm{T}}\right)S_k^{\mathrm{T}}+Q_k\end{array},$$

(39)

(ii) Measurement update

$$\{\begin{array}{l}{x}_{k/k}=x_{k/k-1}+K_k(z_k-z_{k/k-1})\\ K_k=P_{xz,k/k-1}{P}_{zz,k/k-1}^{-1}\\ P_{k/k}=P_{k/k-1}-[P_{xz,k/k-1}{P}_{k/k-1}]{\left[\begin{array}{cc}{P}_{zz,k/k-1}-R_k+I\hfill & P_{xz,k/k-1}^{\mathrm{T}}\\ P_{xz,k/k-1}\hfill & P_{k/k-1}-{\gamma }^{2}I\end{array}\right]}^{-1}\left[\begin{array}{l}{P}_{xz,k/k-1}^{\mathrm{T}}\\ P_{k/k-1}^{\mathrm{T}}\end{array}\right]\end{array}.$$

(40)

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:

$$Z_{\rho }(t)={\rho }_{\mathrm{GPS}}-{\rho }_{\mathrm{INS}},$$

(41)

where ${\rho }_{\mathrm{GPS}}$ and ${\rho }_{\mathrm{INS}}$ are the position and velocity information output by the GPS and INS, respectively. Then the measurement vector is presented as:

$$Z_k=\left[\begin{array}{l}{r}_{\mathrm{GPS}}-r_{\mathrm{INS}}\\ v_{\mathrm{GPS}}-v_{\mathrm{INS}}\end{array}\right],$$

(42)

where $Z_k$ denotes the measurement vector of the integrated navigation system at epoch $k$, $r_{\mathrm{GPS}}$, $r_{\mathrm{INS}}$, $v_{\mathrm{GPS}}$, and $v_{\mathrm{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:

$$\{\begin{array}{l}{x}_{k/k}=x_{k/k-1}+K_k(z_k-H_k{x}_{k/k-1})\\ K_k=P_{k/k-1}{H}_k^{\mathrm{T}}{(H_k{P}_{k/k-1}{H}_k^{\mathrm{T}}+R_k)}^{-1}\\ P_{k/k}=P_{k/k-1}-[P_{k/k-1}{H}_k^{\mathrm{T}}{P}_{k/k-1}]{\left[\begin{array}{ll}{H}_k{P}_{k/k-1}{H}_k^{\mathrm{T}}+I& H_k{P}_{k/k-1}^{\mathrm{T}}\\ P_{k/k-1}{H}_k^{\mathrm{T}}& P_{k/k-1}-{\gamma }^{2}I\end{array}\right]}^{-1}\left[\begin{array}{l}{H}_k{P}_{k/k-1}^{\mathrm{T}}\\ P_{k/k-1}^{\mathrm{T}}\end{array}\right]\end{array}.$$

(43)

Obviously, the observable variables are the position and velocity information of each direction, thus the corresponding fading factors can be estimated adaptively.

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

5.1. 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. Main technological parameters of IMU and GPS.

Sensors	Random Bias	Random Constant Noise	RMSEs
Gyroscope	20 (°)/h	0.067 (°)/h1/2	-
Accelerometer	5 mg	50 μg/h1/2	-
GPS receiver	-	-	Position: 0.5 m; Velocity: 0.05 m/s

.

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² and 0.0025 m²/s², 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:

• Scheme 1: the cubature Kalman Filter (CKF);
• Scheme 2: the H-infinity Filter (HF-CKF) ($\gamma$ was set as 2);
• Scheme 3: the conventional multiple fading filter (MF-CKF);
• Scheme 4: the new multiple fading filter (NMF-CKF);
• Scheme 5: the multiple fading H-infinity filter (MHF-CKF) ($\gamma$ was set as 2);
• Scheme 6: the new multiple fading H-infinity filter (NMHF-CKF) ($\gamma$ was set as 2).

5.2. Results of the Experiments

(1) Case 1:

In this case, each algorithm was performed with the original measurement data. The position errors in the X, Y, Z directions of the six schemes are demonstrated in Figure 2, Figure 3, Figure 4, Figure 5, Figure 6 and Figure 7:

The position and attitude RMSEs of the each scheme are listed in

Table 2. Position RMSEs of each scheme (meter).

Axis	CKF	HF-CKF	MF-CKF	NMF-CKF	MHF-CKF	NMHF-CKF
$X$	0.129	0.096	0.108	0.093	0.095	0.088
$Y$	0.284	0.204	0.207	0.181	0.195	0.167
$Z$	0.188	0.128	0.127	0.121	0.126	0.112

and

Table 3. Attitude RMSEs of each scheme (degree).

Axis	CKF	HF-CKF	MF-CKF	NMF-CKF	MHF-CKF	NMHF-CKF
$X$	0.043	0.042	0.042	0.038	0.040	0.036
$Y$	0.051	0.049	0.052	0.042	0.048	0.040
$Z$	0.324	0.278	0.275	0.233	0.243	0.215

, respectively.

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 Figure 2 and Figure 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, Figure 4 and Figure 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 Figure 6 and Figure 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.

(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 Figure 8, Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13:

In this case, the position errors are mainly caused by outliers. It is clearly demonstrated from the Figure 8 and Figure 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 Figure 10, Figure 11, Figure 12 and Figure 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

Table 4. Position RMSEs of each scheme (meter).

Axis	CKF	HF-CKF	MF-CKF	NMF-CKF	MHF-CKF	NMHF-CKF
$X$	0.482	0.257	0.124	0.109	0.114	0.092
$Y$	0.672	0.409	0.224	0.191	0.198	0.178
$Z$	0.488	0.273	0.140	0.127	0.130	0.121

and

Table 5. Attitude RMSEs of each scheme (degree).

Axis	CKF	HF-CKF	MF-CKF	NMF-CKF	MHF-CKF	NMHF-CKF
$X$	0.046	0.044	0.043	0.041	0.042	0.038
$Y$	0.052	0.050	0.056	0.048	0.050	0.046
$Z$	0.326	0.312	0.301	0.254	0.265	0.239

:

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

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