Adaptive H-infinity extended Kalman filtering for a navigation system in presence of high uncertainties

Abstract

The optimal performance of the Kalman filters is highly dependent on the measurement and process noise characteristics, making the whole system unable to achieve the desired estimation in the presence of non-Gaussian mean noise distribution and high initial uncertainties. Recently, the H-infinity filter, as a robust algorithm, has been broadly used, as it is not being dependent on the pre-knowledge of the noise nature; however, making a balance between high robustness and estimation accuracy is a challenging issue. Hence, to overcome this problem, a new adaptive H-infinity extended Kalman filter (AHEKF) was designed in this paper, which benefits from both high robustness and precision. The suggested algorithm contains two adaptive sections to achieve high accuracy as well as controlling the effects of time-varying noise characteristics, high initial uncertainties, and abnormal data that can degrade the accuracy of state estimation in an integrated navigation system. The presented algorithm was used to integrate data from two independent sensors data. The simulation results for an inertial navigation system (INS)/global positioning system (GPS) sensor fusion are presented and compared with the standard H-infinity filter, extended Kalman filter (EKF), and unscented Kalman filter (UKF) to show the effectiveness of the proposed algorithm. Evaluations demonstrate that the AHEKF achieves over 50% higher accuracy and robustness, and over 2.5 times faster convergence of estimation errors than the standard H-infinity filter.

Introduction

Inertial navigation system (INS) and global positioning system (GPS) are extensively used for positioning and attitude determination for aircraft and vehicle navigation or tracking, marine applications, and so on. It is well known that GPS-like signals are susceptible to jamming or being lost due to electromagnetic wave limitation (Liu et al., 2018). In addition, all INSs suffer from integration drift; small errors in the measurement of angular velocity and acceleration are integrated into larger errors in velocity and position (Hu et al., 2015; Sabzevari and Chatraei, 2021). Due to these limitations, the GPS/INS integration provides more accurate performance in comparison with GPS or INS standalone (Wang et al., 2006). Efficient sensor fusion algorithms and methods based on Kalman filter were proposed (Sasiadek and Wang, 2003). To improve the navigation system accuracy, extensive researches have been done to fuse the information of data from different sensors based on the Kalman filters (KFs) approach (Liu et al., 2018; Van Der Merwe et al., 2004). The divergence problem of standard KFs due to poor knowledge of the models or non-Gaussian noises are the most challenging issue (Hu et al., 2015; Huang et al., 2017; Wang et al., 2017). Unlike the standard KFs, which require detailed knowledge of the noise, the H-infinity filters require no prior assumption of process and measurement noise (Simon, 2006). As a result, it is a more efficient algorithm than standard KFs (Gong et al., 2020). Multitude types of H-infinity filter solutions have been considered such as game theory (Shen and Deng, 1997), frequency domain (Grimble and El Sayed, 1990), and Krein -space approach in terms of the state-space form (Hassibi et al., 1999). Although the Krein space approach is used for linearized systems, it can be utilized for the nonlinear system based on the extended Kalman filter (EKF) (Zhao, 2017). Despite the good performance of H-infinity filters, there are many circumstances such as model uncertainty, and environmental disturbances, resulting in information fusion hindrance and low accuracy of estimations (Havangi, 2015; Yu et al., 2014). In addition, inaccurate estimation of initial values can make the filter unstable. To manage these limitations, adaptive approaches have been proven to be an effective strategy (Yang and Gao, 2006). Multiple types of adaptive formation have been studied like innovation and residual-based adaptive KFs (Aghili and Su, 2016; Tong et al., 2017), in which only the adaption of the measurement noise and gain matrix is considered. Some others like Shao et al. (2016) use adaptive EKF, in which both measurement and process covariance matrices are considered as adaptive ones. Although the multiple-model-based adaptive estimation (MMAE) can somehow overcome the model uncertainty, due to implementing multiple banks of KFs, the computational burden would impact the system performance (Kottath et al., 2017). The adaptive fuzzy algorithm is proposed in Assad et al. (2019), Yazdkhasti and Sasiadek (2018), and Yazdkhasti et al. (2016), which is based on EKF, weighted EKF, and unscented Kalman filter (UKF), improving the estimation accuracy by adaption of the process ( ${\mathit{Q}}_k$ ) and measurement ( ${\mathit{R}}_{\mathit{k}}$ ) covariance matrices. Despite the advantages of the H-infinity filters such as robustness and independence of initial noise knowledge, for the optimal results, more parameters need to be tuned, and high accuracy of the filter cannot obtain in some cases (Wang et al., 2017; Yu et al., 2014). Therefore, some adaptive form of the H-infinity extended Kalman filter (HEKF) has been considered to address these problems. A fuzzy adaptive H-infinity filter, an adaptive fuzzy hybrid UKF, and a robust H-infinity filter are considered in Liu et al. (2008), Tehrani et al. (2019), and Zhao and Mili (2019), in which a proper weight, the threshold value (γ), and the error covariance matrix ( ${\mathit{P}}_k$ ) are assumed to be tuned; however, the adaption of ${\mathit{R}}_k$ and ${\mathit{Q}}_k$ are not considered, which can affect the system accuracy when noise property changed. Furthermore, some others used an adaptive method for regulation γ and/or a tuning parameter to control both robustness and accuracy; however, outliers in measurement or variation in noise characteristics can affect the system performance (Lyu and Cheng, 2017; Zhao et al., 2018). Also, strict variation of this threshold is not considered which could increase or decrease the covariance matrix abruptly, reducing the system stability and accuracy noticeably.

Therefore, to tackle the aforementioned problems, in this paper a novel adaptive H-infinity extended Kalman filter (AHEKF) is designed. The contribution of this paper is as follows: First, an adaptive factor is assumed to tune ${\mathit{P}}_k$ accurately, while a suitable value is considered for γ to make the system robust enough. This can help the system to avoid possible instability and inaccuracy due to the strict variation of this threshold in the adaptive structures applied by previous works. Second, due to the high initial uncertainties, unknown or time-varying noise characteristics, ${\mathit{Q}}_k$ and ${\mathit{R}}_k$ weights are presumed to be simultaneously and adaptively tuned-up based on the innovation and residual vector, respectively, to achieve optimal performance as well. Therefore, all covariance matrices are under the supervisory of the adaptive structure to make a correct tradeoff between accuracy and robustness. Third, a judgment criterion is considered by upper and lower bound levels when the residual vector is contaminated by environmental disturbances like outliers caused by GPS interference. Finally, the performance of AHEKF in terms of lower estimation error, higher robustness, and precision is evaluated by considering the effect of the non-zero mean noises, high initial uncertainties, and outliers, then compared with EKF, UKF, and HEKF in each section.

The rest of the paper is structured as follows: The second section represents the principle of the HEKF. In the third section, the adaptive structure is proposed in which an adaptive scale factor is used to converge the effect of non-zero mean noises more rapidly. In addition, the adaption of ${\mathit{R}}_k$ and ${\mathit{Q}}_k$ weights that can control the influence of high uncertainties and disturbances in the navigation system is presented. In the last section, four schemes are considered, and simulations demonstrate the performance of AHEKF, HEKF, UKF, and EKF against various conditions such as robustness against high initial uncertainties, non-zero mean noise, and measurement outlier data.

The H-infinity extended Kalman filter

The HEKF makes robust estimations against environmental noises. As a result, it is widely used in industrial applications (Grimble and El Sayed, 1990; Shen and Deng, 1997; Simon, 2006). Unlike the standard KFs, the HEKF requires no primary assumption of the noise characteristics. The KFs minimize the mean squared error of the estimation error, while the HEKF minimizes the highest estimation error, which makes it more robust than the various KFs such as EKF and UKF (Hassibi et al., 1996, 1999; Zhao, 2017). The nonlinear system model is defined as follows (Hassibi et al., 1996; Simon, 2006)

$${\mathit{x}}_{k|k}=f\left(x_{k|k-1}\right)+{\mathit{G}}_k{\mathit{w}}_k$$

(1)

$${\mathit{y}}_{k|k}=h\left(x_{k|k}\right)+{\mathit{v}}_k$$

(2)

where ${\mathit{x}}_{k|k}$ and ${\mathit{y}}_{k|k}$ are state and measurement vectors, respectively. ${\mathit{v}}_k$ is measurement noise and ${\mathit{w}}_k$ represents the process noise with ${\mathit{R}}_k$ and ${\mathit{Q}}_k$ covariance matrices, respectively (Hu et al., 2015; Huang et al., 2017). The ${\mathit{G}}_k$ is a known matrix. The $f(.)$ and $h(.)$ are nonlinear functions, which are used for estimating the predicted state and measurement (Yazdkhasti and Sasiadek, 2018). Although $f(.)$ and $h(.)$ would not be explicitly used in the update step of the EKF, the Jacobian format is used as transition and observation matrices, respectively (Liu et al., 2018; Van Der Merwe et al., 2004)

$${\mathit{F}}_k=\frac{\partial f}{\partial \mathit{x}}{|}_{{\hat{\mathit{x}}}_{k-1|k-1}}$$

(3)

$${\mathit{H}}_k=\frac{\partial h}{\partial \mathit{x}}{|}_{{\hat{\mathit{x}}}_{k|k-1}}$$

(4)

The HEKF would not only finite the upper bound on estimation error, but also minimize this upper bound. Following this, a cost function is defined as follows (Hassibi et al., 1999; Simon, 2006; Zhao and Mili, 2019)

$$J=\frac{{\sum }_{k=1}^N{\mathit{x}}_k-{{\hat{\mathit{x}}}_k}^2}{{\mathit{x}}_0-{{\hat{\mathit{x}}}_0}_{{\mathit{P}}_0^{-1}}^2+{\sum }_{k=1}^N({{\mathit{w}}_k}_{{\mathit{Q}}_k^{-1}}^2+{{\mathit{v}}_k}_{{\mathit{R}}_k^{-1}}^2)}$$

(5)

where ${\mathit{x}}_0$ is the initial state vector, ${\hat{\mathit{x}}}_0$ is its estimate, and ${\hat{\mathit{x}}}_k$ denotes the estimate of ${\mathit{x}}_k$ . N represents the number of the filter iteration and ${\mathit{P}}_0$ is the initial covariance of ${\mathit{x}}_k$ . ${\mathit{P}}_0$ , ${\mathit{Q}}_k$ , and ${\mathit{R}}_k$ are considered symmetric and Positive Definite Matrix (PDM) which must be selected by the designer. The goal is to find ${\hat{\mathit{x}}}_k$ which minimizes the cost function $(J)$ . However, to have an optimal HEKF estimation, the explicit minimization of J is analytically crucial to obtain. Therefore, various sub-optimal methods such as the Krein space approach are considered to address the H-infinity problems by setting the threshold as a performance bound to the cost function as follows (Hassibi et al., 1999; Simon, 2006; Zhao, 2017)

$$\mathrm{su}{p}_{\{{\mathit{x}}_0,{\mathit{v}}_k,{\mathit{w}}_k\}}\frac{{\sum }_{k=1}^N{\mathit{x}}_k-{{\hat{\mathit{x}}}_k}^2}{{\mathit{x}}_k-{{\hat{\mathit{x}}}_k}_{{\mathit{P}}_0^{-1}}^2+{\sum }_{k=1}^N({{\mathit{w}}_k}_{{\mathit{Q}}_k^{-1}}^2+{{\mathit{v}}_k}_{{\mathit{R}}_k^{-1}}^2)}\le {\gamma }^2$$

(6)

where $\gamma >0$ and ${\gamma }^2$ denotes the threshold value that limits the upper bound energy of the cost function. The smaller $\gamma$ selected, the more robust estimator will have. Also, if $\gamma$ →∞, the adaption of covariance matrices plays a crucial role in designing a robust and accurate filter. A discrete state-space model of the standard HEKF can be written as (Hassibi et al., 1996; Simon, 2006)

$${\mathit{x}}_{k|k}={\mathit{F}}_k{\mathit{x}}_{k|k-1}+{\mathit{G}}_k{\mathit{w}}_k$$

(7)

$${\mathit{y}}_{k|k}={\mathit{H}}_k{x}_{k|k}+{\mathit{v}}_k$$

(8)

$${\mathit{z}}_{k|k}={\mathit{L}}_k{\mathit{x}}_{k|k}$$

(9)

where ${\mathit{z}}_{k|k}$ is a linear combination of states, and ${\mathit{L}}_k$ is defined as an identity matrix $({\mathit{L}}_k=\mathit{I})$ as the estimation errors are assumed to have equal weights (Gong et al., 2020).

Lemma 1 (Hassibi et al., 1996): the HEKF would not have a solution unless $\gamma >0$ , and $[{\mathit{F}}_k{\mathit{G}}_k]$ is a full ranked matrix and the following inequality is satisfied for all steps (k)

$${\mathit{P}}_{k|k}^{-1}+{\mathit{H}}_k^T{\mathit{R}}_k^{-1}{\mathit{H}}_k-{\gamma }^{-2}{\mathit{L}}_{\mathit{k}}^{\mathit{T}}{\mathit{L}}_{\mathit{k}}>0$$

(10)

where ${\mathit{R}}_k^{-1}$ is the inverse of the measurement covariance matrix, and the Riccati recursive relation will be obtained from ${\mathit{P}}_{k|k-1}$ as follows

$${\mathit{P}}_{k|k}={\mathit{P}}_{k|k-1}-{\mathit{P}}_{k|k-1}\left[\begin{array}{cc}{\mathit{H}}_k^T\hfill & {\mathit{L}}_k^T\hfill \end{array}\right]{\mathit{T}}_{e,k}^{-1}\left[\begin{array}{c}{\mathit{H}}_k\hfill \\ \multicolumn{1}{c}{{\mathit{L}}_k}\end{array}\right]{\mathit{P}}_{k|k-1}$$

(11)

where ${\mathit{P}}_{k|k-1}$ and the Krein space Gramian ( ${\mathit{T}}_{e,k}$ ) are defined as

$${\mathit{P}}_{k|k-1}={\mathit{F}}_k{\mathit{P}}_{k-1|k-1}{\mathit{F}}_k^T+{\mathit{Q}}_k$$

(12)

$${\mathit{T}}_{e,k}=\left[\begin{array}{cc}{\mathit{R}}_k+{\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T\hfill & {\left({\mathit{P}}_{k|k-1}{\mathit{H}}_k^T\right)}^T\hfill \\ \multicolumn{1}{c}{{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T}& -{\gamma }^{-2}\mathit{I}+{\mathit{P}}_{k|k-1}\hfill \end{array}\right]$$

(13)

By attending $\gamma$ →∞ the Riccati recursion of the HEKF is acting like a regular EKF (Simon, 2006; Tehrani et al., 2019). The HEKF gain and estimation of state vector can be written as

$${\mathit{K}}_{\mathit{k}}={\mathit{P}}_{k|k-1}{\mathit{H}}_k^T{({\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T+{\mathit{R}}_k)}^{-1}$$

(14)

$${\hat{\mathit{x}}}_{k|k}={\hat{\mathit{x}}}_{k|k-1}+{\mathit{K}}_k({\mathit{y}}_k-{\mathit{H}}_k{\hat{\mathit{x}}}_{k|k-1})$$

(15)

where ${\hat{\mathit{x}}}_{k|k}$ is the estimation of ${\mathit{x}}_{k|k}$ .

In this case, a Krein space model as a sub-optimal solution is used to address the optimal H-infinity problem as its analytical solution was a complicated method to achieve (Havangi, 2015; Shen and Deng, 1997; Simon, 2006). Also, the threshold value should be tuned precisely, since inaccurate tuning may cause the filter divergence; however, there is no actual upper level for this threshold. Hence, to maintain both robustness and accuracy, in this paper, an adaptive algorithm is designed to overcome these issues and make estimations more accurate and flexible by tuning the error covariance matrix as well as ${\mathit{Q}}_k$ and ${\mathit{R}}_k$ weights to avoid unknown or time-varying noise characteristics, high initial uncertainties, and outliers.

The adaptive H-infinity extended Kalman filter

Various kinds of adaptive and robust KFs have been studied (Kottath et al., 2017; Wang et al., 2006; Yang et al., 2001). In most cases, the suggested algorithm was neither accurate nor robust enough to deal with system model uncertainties, non-Gaussian noise distribution, and outliers (Liu et al., 2008; Shao et al., 2016). Therefore, in this paper, to overcome the influence of abnormal observations in measurement, noise characteristics variation, system instability, and inaccuracy, the AHEKF is designed. The AHEKF algorithm consists of two adaption sections: first, an adaptive scale factor based on HEKF, which can converge the effect of non-zero mean noises more rapidly; second, the adaption of ${\mathit{R}}_k$ and ${\mathit{Q}}_k$ weights, which can control the influence of high uncertainties and disturbances in the navigation system.

Adaptive scale factors

In this section, the proposed AHEKF is developed by using an adaptive scale factor $({\sigma }_k)$ in error covariance matrix ( ${\mathit{P}}_{k|k}$ ) to make a tradeoff between robustness and accuracy of the filter, which might not applicable when using directly adaptive $\gamma$ . To evaluate the system errors, a residual vector $({\mathit{V}}_k)$ is defined as (Assad et al., 2019; Yang et al., 2001)

$${\mathit{V}}_k={\mathit{y}}_k-{\mathit{H}}_k{\hat{\mathit{x}}}_{k|k}$$

(16)

In addition, the covariance matrix of the residual vector is defined as (Aghili and Su, 2016; Sabzevari and Chatraei, 2021)

$${\mathit{S}}_k={\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T+{\mathit{R}}_k$$

(17)

By using the covariance matrix of the residual, the adaptive factor is defined as

$${\sigma }_k=\{\begin{array}{c}\frac{c_1}{\left|\mathrm{\Delta }{V}_k\right|}\left|\mathrm{\Delta }{V}_k\right|>c_1\hfill \\ \multicolumn{1}{c}{1\left|\mathrm{\Delta }{V}_k\right|\le c_1}\end{array}$$

(18)

where $c_1$ is a positive constant value between [1, 2.5] (Yang and Gao, 2006), and $\left|\mathrm{\Delta }{V}_k\right|$ is defined as

$$\left|\mathrm{\Delta }{V}_k\right|=\sqrt{\left(\frac{{\mathit{V}}_k^T{\mathit{V}}_k}{\mathrm{trace}({\mathit{S}}_k)}\right)}$$

(19)

Consequently, the adaptive scale factor can be employed as follows

$${\mathit{P}}_{k|k}=\frac{1}{{\sigma }_k}{\mathit{P}}_{k|k-1}-\frac{1}{{\sigma }_k}{\mathit{P}}_{k|k-1}\left[\begin{array}{cc}{\mathit{H}}_k^T\hfill & {\mathit{L}}_k^T\hfill \end{array}\right]{\mathit{T}}_{e,k}^{-1}\left[\begin{array}{c}{\mathit{H}}_k\hfill \\ \multicolumn{1}{c}{{\mathit{L}}_k}\end{array}\right]{\mathit{P}}_{k|k-1}$$

(20)

where ${\mathit{T}}_{e,k}^{-1}$ is the inverse of ${\mathit{T}}_{e,k}$ and defined as

$${\mathit{T}}_{e,k}=\left[\begin{array}{cc}{\mathit{R}}_k+{\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T\hfill & {\left({\mathit{P}}_{k|k-1}{\mathit{H}}_k^T\right)}^T\hfill \\ \multicolumn{1}{c}{{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T}& -{\gamma }^{-2}\mathit{I}+{\mathit{P}}_{k|k-1}\hfill \end{array}\right]$$

(21)

Therefore, in this section, AHEKF is developed based on a scale factor $({\sigma }_k)$ to control the tradeoff between robustness and accuracy of the filter by tuning the filter covariance matrix. However, unforeseen circumstances such as high initial uncertainties, time-varying noise characteristics, and outlier data are inevitable ones which can be handled to some extent and maintain the accuracy of the system by adapting process and measurement covariance.

R_k and Q_k adaption weights

Although HEKF does not require knowing the nature of the measurement and process noise, for optimal performance the ${\mathit{R}}_k$ and ${\mathit{Q}}_k$ are needed to be tuned as well (Sabzevari and Chatraei, 2021; Tong et al., 2017; Yu et al., 2014). Tuning these parameters can be challenging as there is a tradeoff between the robustness and accuracy of the filter, especially when there are variations in noise characteristics or high initial uncertainty, and abnormal observation. To perform the ${\mathit{Q}}_k$ and ${\mathit{R}}_k$ adaption at the same time, the adaptation method presented here estimates the ${\overline{\mathit{Q}}}_k$ matrix based on the innovation covariance which is the difference between the actual measurement and its predicted value, and the adaptation method for estimating the ${\overline{\mathit{R}}}_k$ matrix is the residual covariance-based scaling method which is the difference between actual measurement and its estimated value using the information available at step k. Therefore, the adaptive structure of the measurement covariance matrix is considered as follows

$${\overline{\mathit{R}}}_k=(1-{\mu }_k){\mathit{R}}_{k-1}+{\mu }_k({\mathit{V}}_k{\mathit{V}}_k^T+{\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T)$$

(22)

where ${\mathit{V}}_k$ is the residual vector mentioned in equation (16), and ${\mu }_k$ as the tuning parameter defined as

$${\mu }_k=\frac{1-\delta }{(1-{\delta }^k)}$$

(23)

where $\delta$ is a forgetting factor $(0<\delta <1)$ which can be set by experience. In addition, to avoid disturbing data, we can control the judgment criteria by upper and lower bound quantities as

$$L_b{\mathit{R}}_{k-1}<<({\mathit{V}}_k{\mathit{V}}_k^T+{\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T)<<U_b{\mathit{R}}_{k-1}$$

(24)

where $L_b$ and $U_b$ denote lower and upper bounds criteria, respectively. Consequently, the measurement covariance matrix is adaptively tuned, and disturbed or abnormal data will be bounded.

Correspondingly, the process covariance matrix will be modified if errors are intensified significantly (Shao et al., 2016). Therefore, the adaption of ${\mathit{Q}}_k$ is defined as

$${\overline{\mathit{Q}}}_k=\{\begin{array}{c}{\mu }_k({\mathit{K}}_k{\overline{\mathit{V}}}_k{\overline{\mathit{V}}}_k^T{\mathit{K}}_k^T+{\mathit{P}}_{k|k-1}-{\mathit{F}}_k{\mathit{P}}_{k-1|k-1}{\mathit{F}}_k^T)\hfill \\ \multicolumn{1}{c}{+(1-{\mu }_k){\mathit{Q}}_{k-1},\left|{\overline{\mathit{V}}}_k^T{\mathit{S}}_k{\overline{\mathit{V}}}_k\right|>c_2}\\ \multicolumn{1}{c}{{\mathit{Q}}_{k-1},\mathrm{otherwise}}\end{array}$$

(25)

where the innovation vector is defined as ${\overline{\mathit{V}}}_k={\mathit{y}}_k-{\mathit{H}}_k{\hat{\mathit{x}}}_{k|k-1}$ and $c_2$ is a positive value which is obtained based on the expectation values of ${\overline{\mathit{V}}}_k^T{\overline{\mathit{V}}}_k$ . Considering this tuning parameter, ${\overline{\mathit{Q}}}_k$ can be adaptively tuned when abnormal changes are detected. Consequently, the recursive form of the new adaptive H-infinity EKF can be written as follows. First, the time update phase can be achieved as

$${\hat{\mathit{x}}}_{k|k-1}={\mathit{F}}_k{\hat{\mathit{x}}}_{k-1|k-1}$$

(26)

$${\mathit{P}}_{k|k-1}={\mathit{F}}_k{\mathit{P}}_{k-1|k-1}{\mathit{F}}_k^T+{\overline{\mathit{Q}}}_k$$

(27)

Second, the measurement update phase based on the residual provided by new measurements can be obtained, which involves the adaptive factors

$${\mathit{K}}_k={\mathit{P}}_{k|k-1}{\mathit{H}}_k^T{({\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T+{\overline{\mathit{R}}}_k)}^{-1}$$

(28)

$${\hat{\mathit{x}}}_{k|k}={\hat{\mathit{x}}}_{k|k-1}+{\mathit{K}}_k({\mathit{y}}_k-{\mathit{H}}_k{\hat{\mathit{x}}}_{k|k-1})$$

(29)

$${\mathit{P}}_{k|k}=\frac{1}{{\sigma }_k}{\mathit{P}}_{k|k-1}-\frac{1}{{\sigma }_k}{\mathit{P}}_{k|k-1}\left[\begin{array}{cc}{\mathit{H}}_k^T\hfill & {\mathit{L}}_k^T\hfill \end{array}\right]{\mathit{T}}_{e,k}^{-1}\left[\begin{array}{c}{\mathit{H}}_k\hfill \\ \multicolumn{1}{c}{{\mathit{L}}_k}\end{array}\right]{\mathit{P}}_{k|k-1}$$

(30)

where ${\mathit{T}}_{e,k}^{-1}$ is the inverse of ${\mathit{T}}_{e,k}$ and defined as

$${\mathit{T}}_{e,k}=\left[\begin{array}{cc}{\overline{\mathit{R}}}_k+{\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T\hfill & {\left({\mathit{P}}_{k|k-1}{\mathit{H}}_k^T\right)}^T\hfill \\ \multicolumn{1}{c}{{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T}& -{\gamma }^{-2}\mathit{I}+{\mathit{P}}_{k|k-1}\hfill \end{array}\right]$$

(31)

Finally, this adaptive factor will benefit the proposed algorithm to avoid the influence of high initial uncertainties and variation of noise nature in the navigation system. In addition, the block diagram of the proposed AHEKF method is presented in Figure 1 to demonstrate the whole structure.

Figure 1.

The block diagram of AHEKF.

Results

The simulation experiment has been done to evaluate the performance of the proposed AHEKF. The state vectors used in the simulation included three states for INS position errors, three for INS velocity errors, and two states for GPS range drift and range bias (Yazdkhasti and Sasiadek, 2018). The initial conditions were selected as follows

$${\hat{\mathit{x}}}_0={[0,0,0,0,0,0,0,0]}^T$$

(32)

Pseudo range equations were implemented as a measurement model of the AHEKF

$${\mathit{h}}_j=\sqrt{{(X_j-x)}^2+{(Y_j-y)}^2+{(Z_j-z)}^2}+c\mathrm{\Delta }{t}_j$$

(33)

where $(X_j,Y_j,Z_j)$ , for j = 1, …, 4 represent the positions of the four GPS satellites, respectively, and $(x,y,z)$ are the position of the autonomous vehicle, $\mathrm{\Delta }{t}_j$ is a receiver offset from the $j_{\mathrm{th}}$ satellite, and $c$ is the speed of light. The non-zero mean noise $w_k$ was chosen to be

$${\mathit{w}}_k={[0,T{\sigma }_x,0,T{\sigma }_y,0,T{\sigma }_z,c{S}_{f}T+\frac{c{S}_g{T}^2}{2},c{S}_{g}T]}^T$$

(34)

With ${\sigma }_x=0.0006$ , ${\sigma }_y=0.0006$ , and ${\sigma }_z=0.0006$ . $T$ is the sample time, $S_f=0.4(10{)}^{-18}$ , is the standard deviation of clock offset, and $S_g=1.58(10{)}^{-18}$ is the standard derivation associated with velocity. The noise with a standard deviation of 4 meters was applied to the GPS measurements. It should be mentioned that the initial conditions for all filters assumed the same.

The AHEKF parameters are designed based on experience. To have both minimum variance and high robustness, $\gamma$ is set to 1.53. The adaptive scale factor can be tuned by $c_1$ in equation (18) is set as $c_1=1.25$ . Likewise, to adapt ${\mathit{Q}}_k$ , $c_2$ the threshold value in equation (25) is set to $0.89$ .

The simulations were performed based on a laptop that was equipped with a Core-i7 CPU, 8GB of RAM, and MATLAB2019a software. The time comparison has been compared between four different KF types involving AHEKF, UKF, EKF, and HEKF. The results are shown in Table 1.

Table 1.

Time consumption comparison.

Method	AHEKF	HEKF	EKF	UKF
Time consumptions (s)	6.36	5.32	4.05	9.89

It is obvious from Table 1 that EKF has the lowest time consumption in contrast to UKF, which has the highest one. The time consumption of HEKF is a bit higher than EKF and lower than AHEKF, which lasts 6.36 seconds. The reason that AHEKF lasts longer than HEKF is due to the computation of the adaption sections. Although AHEKF does not have the lowest running time, it has still much lower time consumption than UKF. To evaluate the algorithm in more details, four different schemes are considered to make an accurate comparison between different KF algorithms.

Scheme 1: Robustness against high initial uncertainties

In this case, the performance of four different filters affected by high initial uncertainties is considered, in which ${\mathit{R}}_k$ and ${\mathit{Q}}_k$ matrices in HEKF, EKF, and UKF are assumed constant, while in AHEKF they can be tuned by equations (22) and (25), respectively.

To evaluate the performance of the proposed AHEKF, all filters have been tested for various parameter uncertainties. The effects of four different uncertainty values on the filter’s variance are shown in Figure 2 which illustrates the variance of AHEKF, UKF, EKF, and HEKF when the highest uncertainty, 3 ${\mathit{Q}}_{k-1}$ and 6 ${\mathit{R}}_{k-1}$ , applied.

Figure 2.

Position variance comparison: (a1): $P_x$ , (a2): $P_y$ , and (a3): $P_z$ when ${\overline{\mathit{Q}}}_k=3{\mathit{Q}}_{k-1}$ and ${\overline{\mathit{R}}}_k=6{\mathit{R}}_{k-1}$ .

3 ${\mathit{Q}}_{k-1}$ and 6 ${\mathit{R}}_{k-1}$ mean the real-time parameters are 3 and 6 times as large as the designed ${\mathit{Q}}_{k-1}$ and ${\mathit{R}}_{k-1}$ . As it shows the AHEKF performs the best in comparison with other filters. As it is shown, the variance converges to a value close to zero faster than others. In addition, HEKF shows better performance and faster convergence rather than EKF and UKF. Although HEKF is a robust algorithm to overcome uncertainties, AHEKF demonstrates more robust performance with lower estimation error.

The simulation was repeated for different covariance values ( ${\mathit{Q}}_{k-1}$ , 3 ${\mathit{R}}_{\mathit{k}-1}$ ) shown in Figure 3(a1)–(a3), and (3 ${\mathit{Q}}_{k-1}$ , ${\mathit{R}}_{k-1}$ ) illustrated in Figure 4(a1)–(a3). The variance when we have ( ${\mathit{Q}}_{k-1}$ , ${\mathit{R}}_{k-1}$ ) presented in Figure 5(a1)–(a3). The filter performance when ${\overline{\mathit{Q}}}_k={\mathit{Q}}_k$ and ${\overline{\mathit{R}}}_k={\mathit{R}}_k$ is approximately the same as illustrated in Figure 5(a1)–(a3); however, the least amount of covariance can be seen from AHEKF due to the adaption of the H-infinity covariance matrix. To be more precise, the AHEKF demonstrated over 2.5 times faster convergence rate of estimation errors than the standard HEKF based on different initial uncertainties. On the whole, for all uncertainty values, results show that the AHEKF performed the best in comparison with UKF, EKF, and HEKF.

Figure 3.

Position variance comparison: (a1): $P_x$ , (a2): $P_y$ , and (a3): $P_z$ when ${\overline{\mathit{Q}}}_k={\mathit{Q}}_{k-1}$ and ${\overline{\mathit{R}}}_k=3{\mathit{R}}_{k-1}$ .

Figure 4.

Position variance comparison: (a1): $P_x$ , (a2): $P_y$ , and (a3): $P_z$ when ${\overline{\mathit{Q}}}_k=3{\mathit{Q}}_{k-1}$ and ${\overline{\mathit{R}}}_k={\mathit{R}}_{k-1}$ .

Figure 5.

Position variance comparison: (a1): $P_x$ , (a2): $P_y$ , and (a3): $P_z$ when ${\overline{\mathit{Q}}}_k={\mathit{Q}}_{k-1}$ and ${\overline{\mathit{R}}}_k={\mathit{R}}_{k-1}$ .

Scheme 2: Robustness against non-zero mean noise

For the second part of the simulation experiment, the performance of all filters is evaluated for different non-zero mean noise values. This set of experiments aims to evaluate the effect of non-zero mean noise on the accuracy of the state estimation. The position errors are presented in Figure 6(a) and velocity errors in Figure 6(b). They represent the corrected position and velocity errors of the AHEKF, HEKF, UKF, and EKF when the noise mean variable is µ = 0.1, as it shows the AHEKF has the lowest position and velocity errors in comparison with other filters. The corrected position and velocity errors of the filters when the noise mean is µ = 0.5 are presented in Figure 7. Also, Figure 8 represents the corrected position and velocity errors of the filters when the non-zero mean is µ = 1.2.

Figure 6.

Errors comparison, when non-zero mean is µ = 0.1: (a) Position errors. (b) Velocity errors.

Figure 7.

Errors comparison, when non-zero mean is µ = 0.5: (a) Position errors. (b) Velocity errors.

Figure 8.

Errors comparison, when non-zero mean is µ = 1.2: (a) Position errors. (b) Velocity errors.

It should be noted that the corrected error is defined as the current INS error minus the estimated INS error. As it illustrates, when the mean increases the position and velocity errors of the proposed AHEKF remain close to zero while the others diverge to high values.

As the non-zero mean noise increases, the adaptive structure would decrease the effect of noise characteristics varying. Therefore, when the non-zero mean noise increases, AHEKF shows higher robustness in the position error by over 50% improvement rather than the classic HEKF procedure and over 120% for EKF and UKF methods. It can be concluded from this section that, AHEKF performs the best compared with other procedures to deal with the non-zero mean noise caused by model uncertainty or biased measurements.

Scheme 3: The worst-case condition

In this section, to appraise the algorithm efficiency, both high initial uncertainties and non-zero mean noise are simultaneously assumed as the initial condition. Following this, multiple cases have been evaluated in which different coefficients of initial process and measurement covariance matrices, and various means of non-zero mean noises are considered as well. Figures 9 to 10 illustrate the position and velocity Root mean square (RMS) errors in all X, Y, and Z directions, separately. In position errors, it is obvious that when ${\overline{\mathit{Q}}}_k=3{\mathit{Q}}_{k-1}$ , ${\overline{\mathit{R}}}_k=3{\mathit{R}}_{k-1}$ , and $\mathrm{\mu }=0.1$ , the RMS error in all directions have the minimum value, while it rose remarkably when ${\overline{\mathit{Q}}}_k={\mathit{Q}}_{k-1}$ , ${\overline{\mathit{R}}}_k=3{\mathit{R}}_{k-1}$ , and $\mathrm{\mu }=1.2$ . A similar trend can be observed in velocity errors in all directions. Although the HEKF has a lower RMS position error than EKF and UKF, the AHEKF demonstrated the least RMS error in either position or velocity. It is evident that when the non-zero mean noise increases, the RMS error in both position and velocity follows an increment trend. To be more precise, in a similar condition (when $\mathrm{\mu }=0.1$ and different high initial values) the maximum position error increment of AHEKF was only 16%, while for HEKF was 34%, for UKF was 67%, and for EKF was 70%.

Figure 9.

RMS errors comparison in X-direction, when both non-zero means and high initial uncertainties are applied: (a): X-Position, (b): X-Velocity.

Figure 10.

RMS errors comparison in Y-direction, when both non-zero means and high initial uncertainties are applied: (a): Y-Position, (b): Y-Velocity.

Figure 11.

RMS errors comparison in Z-direction, when both non-zero means and high initial uncertainties are applied: (a): Z-Position, (b): Z-Velocity.

Scheme 4: Robustness against measurement outlier data

In this section, since the measurement matrix can be affected by disturbing data from environmental observations in the navigation system, an adaptive structure of the measurement covariance matrix $({\overline{\mathit{R}}}_k)$ is considered to bound the measurement outlier data effectively.

Therefore, the AHEKF was not affected as other procedures and maintained the lowest RMS errors which indicates that when we have both non-zero mean noise and high initial conditions, it is essential to adapt both weights ${\mathit{Q}}_k$ and ${\mathit{R}}_k$ as in AHEKF to handle the position errors more effective than others.

As discussed before, the measurement covariance matrix is adaptively tuned as depicted in equation (22), and disturbed or abnormal data will be bounded, which is based on residual covariance monitoring shown in equation (16). The forgetting factor $(\mathrm{\delta })$ in equation (23) is set to 0.95, and the upper $({\mathrm{U}}_{\mathrm{b}})$ and lower $({\mathrm{L}}_{\mathrm{b}})$ bound quantities in equation (24) are set to 0.035 and $1.23$ , respectively. Figures 12 and 13 illustrate the position and velocity errors comparison between EKF, UKF, HEKF, and AHEKF when measurement outlier data are added artificially. As it is shown, the performance of EKF and UKF are approximately the same, while HEKF demonstrated better robustness. Although the HEKF can deal with various uncertainties based on different circumstances, it cannot bound the measurement outlier as well as the proposed AHEKF, which performs the best control on outliers among all other procedures.

Figure 12.

Position errors comparison against outlier measurement data.

Figure 13.

Velocity errors comparison against outlier measurement data.

Conclusion

Although the standard H-infinity filter does not require knowing the nature of the measurement and process noise, for optimal performance in the case of both robustness and accuracy, a novel AHEKF is proposed. The suggested approach can overcome high initial uncertainties, noise characteristics varying, and outlier data in the measurements. Subsequently, an adaptive scale factor is designed, balancing the robustness and accuracy of the filter. Also, in the case of the high value of the initial condition, process and measurement covariance weights are presumed to be adaptively and simultaneously tuned based on the innovation and residual vector, respectively, to achieve optimal performance as well. Finally, a judgment criterion is considered by upper and lower bound levels when the residual vector is contaminated by environmental disturbances. The proposed method is applied to fuse data of GPS and INS sensors, and to evaluate the proposed algorithm performance, four different schemes are presented. The simulation results demonstrated that the AHEKF’s performance was more effective than other compared filters in terms of the faster convergence rate of estimation errors with over 2.5 times improvement in comparison with HEKF, as well as, higher robustness, and higher accuracy, in which the maximum position error increment when applying the worst-case condition for AHEKF was only 16%, while for HEKF was 34%, for UKF was 67%, and for EKF was 70%.