Introduction

Recently, considerable efforts have been made in autonomous driving, as it offers great potential in terms of safety and efficiency. For autonomous applications, navigation systems are indispensable, which shall robustly and accurately estimate the vehicle states, namely position, velocity, and orientation. Further, integrity is also considered an essential requirement for navigation systems in safety–critical applications (Bhatti and Ochieng 2007). Often, a high-rate and highly accurate vehicle-state estimation is achieved by fusing measurements of global navigation satellite systems (GNSS) and inertial navigation systems (INS) using Kalman filtering. In tightly coupled navigation systems, Kalman filter solutions cannot guarantee a reliable vehicle-state estimation under inexact noise statistics. In this case, an extended H∞ filter (EHF) can be used alternatively, outperforming the Kalman filtering regarding system uncertainties (Simon 2006).

Moreover, facing various software and hardware failures in real-world operations (Bhatti and Ochieng 2007), navigation filters need to be extended with an integrity monitoring system. Besides detecting and excluding faulty measurements, the integrity system shall generate a guaranteed protection level (PL), which bounds the estimation error (Groves 2013). Robust filtering of GNSS/INS measurements for accurate state estimation, together with a valid PL generation in the urban environment, remains under investigation and is targeted.

For the state estimation in changing environments, where the modeling of the satellite-measurement noise is often inadequate, a robust approach against environmental disturbances is crucial. Gehrt et al. (2020) have proposed a Bayesian-optimization-based adaptive parameterizing scheme within a Kalman filter, which chooses suitable parameters depending on the operating environment. The usage of this approach is limited to the designed environmental features. Instead of optimizing the parameter anytime and anywhere, robust estimation methods insensitive to environmental changes have been studied. As an alternative to Kalman filters, Simon (2006) has presented the H∞ filtering in detail, which still utilizes the conventional filter structure but shows significantly higher performance under system and environment uncertainties. In the context of GNSS/INS, Osman et al. (2019) have introduced a multi-sensor fusion localization approach using the EHF, where the wheel encoders, GPS, and visual odometry are fused within a loosely coupled structure. For tightly coupled GNSS/INS integration, Jwo and Chen (2003) have demonstrated the robustness of a hybrid H2/H∞ filter against noise variations, assuming that the state transition is of constant-velocity kinematics. More recently, Smolyakov and Langley (2020) have proposed a novel algorithm combining H∞ filter and reinforcement learning for robust positioning solutions in urban environments. Our study focuses on the practical implementation of an EHF on a tightly coupled GNSS/INS system based on a modern strapdown architecture, emphasizing its robustness against various parameterizations and environmental circumstances.

For the PL generation, statistic approaches (Lee et al. 2018; Zhu et al. 2018) are widely applied and studied, assuming that the statistical distribution of the estimation error is known. Groves (2013) has indicated that such error distribution assumptions are difficult to validate. To release the PL generation from the statistical distribution conjecture, Combastel (2003) proposed set-membership approaches, which only require process and measurement errors to be bounded. In the context of GPS navigation, Dbouk and Schön (2018) compared various bounding methods, including zonotope, for GPS integrity and pointed out that the zonotope method describes the uncertainty propagation in a more meaningful manner. Dbouk and Schön (2020) further derived an outlier detection algorithm by applying zonotopes as uncertainty references for GPS integrity monitoring. In terms of zonotopes combined with an estimation observer, Zhang et al. (2020) have developed a robust and interval-based error estimation approach for descriptor systems and validated it in a simulation environment. Shetty and Gao (2021) recently conducted a nonlinear stochastic reachability analysis to predict the state uncertainty bounds for GNSS-based unmanned aerial vehicle (UAV)-navigation, where probabilistic zonotopes, instead of conventional zonotopes, are applied as the set representation. The experimental validation was conducted in a simulated urban environment. More recently, Bhamidipati et al. (2022) applied zonotopes to represent GNSS shadows for 3D-map-aided GNSS localization. The feasibility and performance of using zonotope on GNSS/INS navigation, particularly on tightly coupled systems, remains to be researched.

Therefore, we propose a novel scheme combining the EHF and zonotope within the navigation system, involving a strapdown-based INS tightly coupled with GNSS pseudorange and deltarange observables. First, we use an EHF to estimate the states robustly against inappropriate parametrization of the GNSS measurement noise. Then, we apply zonotope to generate PL for the estimated vehicle states. The evaluation of the proposed methods uses a semi/sub-urban environment dataset from a commercial land vehicle. The EHF is compared with the extended Kalman filter (EKF) regarding the sensitivity of the state estimation accuracy by grid searching a defined parameter space of GNSS measurement noise. Further, the state estimation accuracy of the EHF and EKF is compared by applying the well-known sigma-ε model (Wieser et al. 2005; Breuer et al. 2016; Konrad et al. 2018) with the same GNSS parameters. Regarding the feasibility of applying zonotopes in real-time applications, we first evaluate the computational time of the 3D (for position PL) and 9D (for PL of the full vehicle states) zonotopes and their overestimation effects under various parameters. Finally, we apply the Stanford-ESA Integrity Diagram to validate the zonotope-based position PL generation, checking the consistency of the estimated error bound with the ground truth. The result of zonotope-based PL estimation is briefly compared with the results in (Zhu et al. 2018), where the PL is estimated based on EKF measurement innovation by applying the sigma-ε model.

Methodology

In the methodology part, we present the navigation scheme involving the EHF for the state estimation and zonotope for the PL generation. As shown in Fig. 1, the EHF is embedded into an existing GNSS/INS frame, introduced in the previous fundamental work (Konrad et al. 2018) from the same research group, including:

  • The strapdown-based state and covariance prediction by applying the 100 Hz inertial measurement unit (IMU) measurements;

  • The GNSS pre-processing, including the correction of various error terms;

  • The handling of measurements at various rates with delay.

Fig. 1
figure 1

Overview of the designed tightly coupled navigation system, including transitions of the states, their covariances, and error bounds. The blue shadow marks the zonotope-relevant components. \(k\) denotes the index of the time epoch

Full size image

Additionally, GNSS fault detection and exclusion (FDE), proposed in previous works (Liu et al. 2019, 2021), is performed after the correction of GNSS observables, such that a set of fault-free GNSS measurements is fed into the EHF.

The methodology of the EHF and zonotope-based PL generation is divided into two parts. The first part provides fundamental EHF and zonotope theories in the Appendix at the end of the manuscript. It is recommended to read the Appendix if the reader is unfamiliar with the corresponding theories. The second part explains their integration into the tightly coupled GNSS/INS in the rest of this section. A summary of the implementation in pseudocode form is given at the end of this section.

State estimation applying extended H ∞ filter

Applying the EHF to estimate the vehicle’s state, i.e., the three-dimensional position, velocity, and attitude, the following state vector \({\varvec{x}}\) is designed:

$${\varvec{x}}={\left({{\varvec{p}}}_{eA}^{e,T} ~ {{\varvec{v}}}_{eA}^{n,T} ~ {{\varvec{q}}}_{b}^{n,T} ~ {{\varvec{b}}}_{a}^{T} ~ {\boldsymbol{ }{\varvec{b}}}_{g}^{T} ~ {c}_{b} ~ {c}_{d}\right)}^{T}$$
(1)

where the index ‘\(A\)’ stands for the antenna body frame, ‘\(b\)’ for the IMU body frame, ‘\(e\)’ for ECEF coordinates, and ‘\(n\)’ for the navigation frame. \({{\varvec{p}}}_{eA}^{e}\in {\mathbb{R}}^{3\times 1}\) represents the position of the antenna body frame origin in the earth-centered-earth-fixed (ECEF) frame. \({{\varvec{v}}}_{eA}^{n}\in {\mathbb{R}}^{3\times 1}\) is the velocity of the antenna body frame origin with respect to the ECEF frame, expressed in the navigation frame north-east-down (NED) coordinates. A quaternion \({{\varvec{q}}}_{b}^{n}\in {\mathbb{R}}^{4\times 1}\) for the alignment of the IMU body frame with the navigation frame is estimated as the expression of vehicle attitude. Additionally, the accelerometer bias \({{\varvec{b}}}_{a}\) and gyroscope bias \({{\varvec{b}}}_{g}\), each of dimension \({\mathbb{R}}^{3\times 1}\), are part of the state vector, which is necessary for IMU integration. Further, the receiver clock bias \({c}_{a}\) and drift \({c}_{d}\) are estimated, which are essential for tightly coupled GNSS integration.

As shown in Fig. 1, the state vector \({\varvec{x}}\) and its covariance \({\varvec{P}}\) are propagated within a strapdown-based INS at 100 Hz. Konrad et al. (2018) have introduced the prediction of vehicle kinematics by applying the IMU accelerometer and gyroscope measurements and the prediction of IMU bias and receiver clock bias, summarized as (A4). Meanwhile, predicting the covariance by applying (A5) requires the system matrix \({{\varvec{F}}}_{k}\) and the noise shaping matrix \({{\varvec{G}}}_{k}\), the derivation of which is given in (Wendel 2009) or (Groves 2013).

Further, the predicted state vector and covariance are corrected by applying (A6)–(A8) whenever a new set of GNSS measurements is available. The GNSS measurements, i.e., pseudoranges and deltaranges, are introduced in the form of a measurement equation as (A2). By applying such nonlinear measurement equations in EHF, a linearization is required to calculate the measurement Jacobian matrix \({{\varvec{H}}}_{k}\), which is sufficiently described in (Wendel 2009) or (Groves 2013).

Measurement model

The measurement equation of pseudorange is given as follows:

$$\tilde{\rho } = {\varvec{p}}_{es}^{e} - {\varvec{p}}_{eA}^{e} + c_{b} + c_{s} + I_{r} + T_{r} + M_{\rho } + \nu_{\rho }$$
(2)

where \(\widetilde{\rho }\) is the measured scalar pseudorange of one satellite, \({{\varvec{p}}}_{es}^{e}\) and \({{\varvec{p}}}_{eA}^{e}\) represent the position of a satellite (denoted by ‘\(s\)’) and the antenna in ECEF coordinates displayed in ECEF, respectively. The terms \({c}_{b}\), \({c}_{s}\), \({I}_{r}\), \({T}_{r}\), \({M}_{\rho }\), \({\nu }_{\rho }\) in (2) represent the receiver and satellite clock errors, the ionospheric error, the tropospheric error, the multi-path error, and the additive measurement noise, respectively. These error sources are corrected with differential correction data from a base station introduced in “Experiment setup” section.

The deltarange observation is calculated as follows:

$$\widetilde{d}={\left({{\varvec{e}}}_{As}^{n}\right)}^{T}\left({{\varvec{v}}}_{es}^{n}-{{\varvec{v}}}_{eA}^{n}\right)+{c}_{d}+{\nu }_{d}$$
(3)

where \(\widetilde{d}\) is the measured scalar relative velocity between the antenna and satellite, known as the deltarange. The vector \({{\varvec{e}}}_{As}^{n}\) is the direction from the antenna to a satellite. The terms \({{\varvec{v}}}_{es}^{n}\) and \({{\varvec{v}}}_{eA}^{n}\) represent the velocity of the satellite and antenna with respect to the navigation frame, respectively. The terms \({c}_{d}\) and \({\nu }_{d}\) represent the clock drift and measurement noise, respectively.

In practice, the GNSS observables are measured in the antenna body frame, while the strapdown algorithm propagates the system state vector in the IMU body frame, shown in Fig. 1. In the navigation filter, this requires a transformation from the defined vehicle states \({{\varvec{p}}}_{eA}^{e}\) and \({{\varvec{v}}}_{eA}^{n}\) to the required states \({{\varvec{p}}}_{eb}^{e}\) and \({{\varvec{v}}}_{eb}^{n}\) in the IMU body frame, shown as follows:

$${{\varvec{p}}}_{eb}^{e}={{\varvec{p}}}_{eA}^{e}-{{\varvec{C}}}_{n}^{e}{{\varvec{C}}}_{b}^{n}{{\varvec{l}}}_{A}^{b}$$
(4)
$${{\varvec{v}}}_{eb}^{n}={{\varvec{v}}}_{eA}^{n}-{{\varvec{C}}}_{b}^{n}\left({{\varvec{\omega}}}_{eb}^{b}\times {{\varvec{l}}}_{A}^{b}\right)$$
(5)

where \({{\varvec{C}}}_{n}^{e}\) and \({{\varvec{C}}}_{b}^{n}\) denote the direction cosine matrix for the vector rotation among coordinate frames, derived from the estimated antenna position \({{\varvec{p}}}_{eA}^{e}\) and the estimated attitude \({{\varvec{q}}}_{b}^{n}\) (Groves 2013), respectively. The transformation uses the lever arm \({{\varvec{l}}}_{A}^{b}\) of the antenna position in the IMU body frame, measured and given in Section "Experiment setup", and considers the translation velocity of the antenna caused by its rotation \({{\varvec{\omega}}}_{eb}^{b}\), derived from the IMU measurement and estimated IMU bias \({{\varvec{b}}}_{g}\) (Wendel 2009).

Equations (2) and (3) give the measurement equations of a single satellite and are applied to all observed satellites within the EHF as a measurement update step (Wendel 2009; Groves 2013; Konrad et al. 2018).

Error-bound estimation using zonotope

The core issue of the PL calculation is to bound the state estimation error \({{\varvec{e}}}_{{\varvec{k}}}\). Analog to the filter state transition, the estimation of the state error bound [\({{\varvec{e}}}_{{\varvec{k}}}\)] also contains a-priori propagation and a-posterior correction. The error dynamic of the EHF is given as follows:

$${{\varvec{e}}}_{k}^{-}={{\varvec{x}}}_{k}-{\widehat{{\varvec{x}}}}_{k}^{-}={{\varvec{F}}}_{k-1}{{\varvec{e}}}_{k-1}^{+}+{{\varvec{G}}}_{k}{{\varvec{w}}}_{k-1}$$
(6)
$${{\varvec{e}}}_{k}^{+}={{\varvec{x}}}_{k}-{\widehat{{\varvec{x}}}}_{k}^{+}=\left({\varvec{I}}-{{\varvec{K}}}_{k}{{\varvec{H}}}_{k}\right){{\varvec{e}}}_{k}^{-}-{{\varvec{K}}}_{k}{{\varvec{v}}}_{k}$$
(7)

where \({{\varvec{e}}}_{k}^{-}\) and \({{\varvec{e}}}_{k}^{+}\) represent the a-priori and a-posterior estimation error. \({\widehat{{\varvec{x}}}}_{k}^{-}\), \({\widehat{{\varvec{x}}}}_{k}^{+}\), and \({{\varvec{K}}}_{k}\) are filter-relevant variables and defined in (A4)–(A8).

Assume that the initial estimation error \({{\varvec{e}}}_{0}\), the process noise \({{\varvec{w}}}_{k}\) and the measurement noise \({{\varvec{\nu}}}_{k}\) are zero-mean and bounded by corresponding zonotopes:

$${{\varvec{e}}}_{0}\in <\varvec{0},{\varvec{\mathcal{E}}}_{\varvec{0}}>,{{\varvec{w}}}_{k}\in <\varvec{0},{\varvec{\mathcal{W}}}_{k}>,{{\varvec{\nu}}}_{k}\in <\varvec{0},{\varvec{\mathcal{V}}}_{k}>$$
(8)

where \({\varvec{\mathcal{E}}}_{\varvec 0}\), \({\varvec{\mathcal{W}}}_{k}\), and \({\varvec{\mathcal{V}}}_{k}\) represent the generation matrix of corresponding zonotopes, defined in (A12). \({\varvec{\mathcal{E}}}_{\varvec 0}\) shall be initialized large enough, considering the large system uncertainties in the filter initialization stage. \({\varvec{\mathcal{W}}}_{k}\) and \({\varvec{\mathcal{V}}}_{k}\) are calculated as diagonal matrices with their diagonal elements equal to \({n}_{\sigma }\) times the standard deviation of process noise \({\sigma }_{\varvec{{w}}_{k}}\) and measurement noise \({\sigma }_{{{\varvec{\nu}}}_{k}}\), respectively. \({n}_{\sigma }\) is a pre-defined parameter corresponding to the confidence level of the resulting zonotope. Noteworthy, for the calculation of \({\varvec{\mathcal{W}}}_{k}\) and \({\varvec{\mathcal{V}}}_{k}\), we still apply the statistical distribution of the process and measurement noise, which is a zero-mean normal distribution. However, such distribution is not required by the implementing zonotope. Instead, zonotope only requires the lower and upper bound, which can be obtained from the sensor characteristics of the manufacturer or vehicle dynamic limitations. In general, the measurement boundaries are more convenient to obtain and validate in practice, which is one main advantage of set-membership approaches over statistic approaches.

After the zonotope is initialized with \({\varvec{\mathcal{E}}}_{\varvec 0}\), the a-priori and a-posteriori zonotope estimations are derived analogously to (6) and (7). The matrices multiplication and matrices addition in (6) and (7) correspond to linear image \(\odot\) and Minkowski sum \(\oplus\) in the zonotope calculation, explained with (A13) and (A14), respectively:

$$<{{\varvec{c}}}_{{e}_{k}}^{-},{\varvec{\mathcal{E}}}_{k}^{-}>=\left({{\varvec{F}}}_{k-1}\odot <{{\varvec{c}}}_{{e}_{k-1}}^{+},{\varvec{\mathcal{E}}}_{k-1}^{+}>\right)\oplus \left({{\varvec{G}}}_{k-1}\odot <\varvec{0},{\varvec{\mathcal{W}}}_{k-1}>\right)$$
(9)
$$<{{\varvec{c}}}_{{e}_{k}}^{+},{\varvec{\mathcal{E}}}_{k}^{+}>=\left(\left({\varvec{I}}-{{\varvec{K}}}_{k}{{\varvec{H}}}_{k}\right)\odot <{{\varvec{c}}}_{{e}_{k}}^{-},{\varvec{\mathcal{E}}}_{k}^{-}>\right)\oplus \left({{\varvec{K}}}_{k}\odot <\varvec{0},{\varvec{\mathcal{V}}}_{k}>\right)$$
(10)

where \({{\varvec{c}}}_{{e}_{k}}^{-}\), \({{\varvec{c}}}_{{e}_{k}}^{+}\), \({\varvec{\mathcal{E}}}_{k}^{-}\), \({\varvec{\mathcal{E}}}_{k}^{+}\) are the center and generation matrix of the a-priori and a-posteriori estimated bounding zonotope for the state estimation error \({{\varvec{e}}}_{{\varvec{k}}}\). Since \(<{{\varvec{c}}}_{{e}_{k}}^{-},{\varvec{\mathcal{E}}}_{k}^{-}>\) and \(<{{\varvec{c}}}_{{e}_{k}}^{+},{\varvec{\mathcal{E}}}_{k}^{+}>\) are iteratively updated using the zero-mean zonotopes \(<\varvec{0},{\varvec{\mathcal{E}}}_{0}>\), \(<\varvec{0},{\varvec{\mathcal{W}}}_{k}>\), \(<\varvec{0},{\varvec{\mathcal{V}}}_{k}>\), the center of the resulting zonotope \({{\varvec{c}}}_{{e}_{k}}^{-}\) and \({{\varvec{c}}}_{{e}_{k}}^{+}\) shall be zero, which means

$${{\varvec{e}}}_{k}\in <\varvec{0},{\varvec{\mathcal{E}}}_{k}>, \forall k\in \left\{1,\cdots ,N\right\}$$
(11)

Therefore, only the estimation of the generation matrix \({\varvec{\mathcal{E}}}_{k}^{-}\) and \({\varvec{\mathcal{E}}}_{k}^{+}\) is necessary. Equations (9) and (10) result in the following zonotope propagation and update steps:

$${\varvec{\mathcal{E}}}_{k}^{-}=\mathcal{R}\left(\left[\begin{array}{cc}{{\varvec{F}}}_{k-1}{\varvec{\mathcal{E}}}_{k-1}^{+}& {{\varvec{G}}}_{k-1}{\varvec{\mathcal{W}}}_{k}\end{array}\right]\right)$$
(12)
$${\varvec{\mathcal{E}}}_{k}^{+}=\mathcal{R}\left(\left[\begin{array}{cc}\left({\varvec{I}}-{{\varvec{K}}}_{k}{{\varvec{H}}}_{k}\right){\varvec{\mathcal{E}}}_{k}^{-}& {{\varvec{K}}}_{k}{\varvec{\mathcal{V}}}_{k}\end{array}\right]\right)$$
(13)

Each zonotope estimation carries out a zonotope reduction, explained with (A16) and denoted as ‘\(\mathcal{R}\)’. The dimension of the resulting zonotope is specified by a user-defined reduction order \(q~(n\le q\le m)\), namely \(\mathcal{R}\left(\varvec{\mathcal{H}}\right)\in {\mathbb{R}}^{n\times q}\). Without applying the zonotope reduction, the dimension of the generation matrices \({\varvec{\mathcal{E}}}_{k}^{-}\) and \({\varvec{\mathcal{E}}}_{k}^{+}\) increases without limit along the operation time, which is not affordable for computation units.

In the EHF, the state vector \({\varvec{x}}\) contains 18 states, including the quaternion \({{\varvec{q}}}_{b}^{n}\in {\mathbb{R}}^{4\times 1}\). By the estimation of the state error covariance \({\varvec{P}}\), the error of the orientation vector \({\mathbb{R}}^{3\times 1}\) is considered (Wendel 2009), which makes \({\varvec{P}}\in {\mathbb{R}}^{17\times 17}\). Therefore, the error space of the EHF is 17-dimensional. It results in the generation matrix for the EHF \(\varvec{\mathcal{E}}\in {\mathbb{R}}^{17\times q}\). However, in practice, the PLs of some states could be undesired, e.g., PLs of the IMU bias. Suppose that the sequence of the index of the desired PLs in the EHF’s error space is given as follows:

$${\varvec{\uptau}}=\left\{{\uptau }_{1}, {\uptau }_{2}, \cdots , {\uptau }_{n}\right\}$$
(14)

where \(n\) is the dimension of the desired zonotope. A selection matrix \({\varvec{\Gamma}}\in {\mathbb{R}}^{n\times 17}\) can be applied to the generation matrix calculation to achieve the state selection. \({\Gamma }_{{\varvec{i}}{\varvec{j}}}\) denotes the element of the \({\mathrm{i}}^{\mathrm{th}}\) column and \({\mathrm{j}}^{\mathrm{th}}\) raw in \({\varvec{\Gamma}}\) and is given as follows:

$${\Gamma }_{ij}=\left\{\begin{array}{c}1,\, \text{for} \,i=j \in \tau \\ 0, \text{otherweise}\end{array}\right., \forall i\in \left\{1, 2, \cdots , n\right\}, \forall j\in \{1, 2, \cdots , 17\}$$
(15)

The calculation of the zonotope \(<\varvec{0},{\varvec{\mathcal{E}}}_{r,k}>\) to bound the selected state errors is given as follows:

$${\varvec{\mathcal{E}}}_{r,k}^{-}=\mathcal{R}\left(\left[\begin{array}{cc}{\varvec{\Gamma}}{{\varvec{F}}}_{k-1}{{\varvec{\Gamma}}}^{T}{\varvec{\mathcal{E}}}_{r, k-1}^{+}&{\varvec{\Gamma}}\left({{\varvec{G}}}_{k-1}{\varvec{\mathcal{W}}}_{k-1}\right)\end{array}\right]\right)$$
(16)
$${\varvec{\mathcal{E}}}_{r,k}^{+}=\mathcal{R}\left(\left[\begin{array}{cc}{\varvec{\Gamma}}\left({\varvec{I}}-{{\varvec{K}}}_{k}{{\varvec{H}}}_{k}\right){{\varvec{\Gamma}}}^{T}{\varvec{\mathcal{E}}}_{r,k}^{-}&{\varvec{\Gamma}}\left({{\varvec{K}}}_{k}{\varvec{\mathcal{V}}}_{k}\right)\end{array}\right]\right)$$
(17)

In practice, the error bound of the position, velocity, and orientation could be of interest, which can be estimated by the nine-dimensional zonotope given as \({\varvec{\mathcal{E}}}_{{\varvec{r}},9{\varvec{D}}}\in {\mathbb{R}}^{9\times q}\). The zonotope could be further reduced to three-dimensional \({\varvec{\mathcal{E}}}_{{\varvec{r}},3{\varvec{D}}}\in {\mathbb{R}}^{3\times q}\) if only the PL of the position is required. In the present work, the three- and nine-dimensional zonotopes are implemented and discussed, which are achieved by selection matrices \({{\varvec{\Gamma}}}_{3{\varvec{D}}}\in {\mathbb{R}}^{3\times 17}\) and \({{\varvec{\Gamma}}}_{9{\varvec{D}}}\in {\mathbb{R}}^{9\times 17}\):

$${{\varvec{\uptau}}}_{3{\varvec{D}}}=\left\{1, 2, 3\right\}, {{\varvec{\Gamma}}}_{3{\varvec{D}}}=\left[\begin{array}{cc}{{\varvec{I}}}_{3\times 3}& {\varvec{0}}_{3\times 14}\end{array}\right]$$
(18)
$${{\varvec{\uptau}}}_{9{\varvec{D}}}=\left\{1, 2, \cdots , 9\right\}, {{\varvec{\Gamma}}}_{9{\varvec{D}}}=\left[\begin{array}{cc}{{\varvec{I}}}_{9\times 9}& {\varvec{0}}_{9\times 8}\end{array}\right]$$
(19)

These two specific zonotopes will be evaluated in the experiment section.

Protection-level generation and integrity risk

In general, the objective of estimating the PL is to find the boundary of estimation error under an integrity risk:

$${\varvec{PL}}_{k} = \sup \left\{ {\left[ {{\varvec{e}}_{k} } \right]|P\left( {\hat{\user2{x}}_{k} - {\varvec{x}}_{k} \in \left[ {{\varvec{e}}_{k} } \right]} \right) \le 1 - P_{IR} } \right\}$$
(20)

where \({{\varvec{x}}}_{k}\) is the unknown ground truth of the state estimate \(\hat{\user2{x}}_{k}\). \(P_{IR}\) denotes the probability of integrity risk. \({{\varvec{P}}{\varvec{L}}}_{k}\) is the vector calculated for all desired states and corresponds to the dimension of \({\varvec{\uptau}}\).

Assuming that (8) is satisfied, (11) indicates that the estimation error must be within the zonotope given as \(<\varvec{0},{\varvec{\mathcal{E}}}_{k}>\) and, therefore, must be within its interval hull. Thus, we estimate the desired protection level as the interval hull box of the resulting reduced zonotope \(<\varvec{0},{\varvec{\mathcal{E}}}_{r,k}>\) from (16) and (17) by applying (A15). In practice, specific PLs could be desired, e.g., horizontal PL (HPL) and vertical PL (VPL) for positioning. The HPL is calculated as the diagonal distance of the interval hull box corresponding to positioning error in the north and east direction, while the VPL is the interval hull of positioning error in the down direction.

The risk of initializing the zonotopes \(<\varvec{0},{\varvec{\mathcal{E}}}_{0}>\), \(<\varvec{0},{\varvec{\mathcal{W}}}_{k}>\), and \(<\varvec{0},{\varvec{\mathcal{V}}}_{k}>\) can be quantified with the pre-defined parameter \({n}_{\sigma }\). However, the zonotope operations sustainably change the integrity risk, such as Minkowski sum, or even deterministically reduce the integrity risk, such as zonotope reduction and interval hull generation. Since the zonotope is independent of the statistical distributions after being initialized with (8), the resulting integrity risk \({P}_{IR}\) is hard to analyze in theory. We evaluate the integrity risk of zonotope-based protection-level generation by applying the Stanford-ESA diagram with recorded real-world data.

Summary of the implementation scheme

The integration of the zonotope into the filter structure is summarized as Alg. 1. It should be noted that Alg. 1 is a general expression that does not require the applied filter to be the EHF. Any other form of filter, e.g., the EKF or the unscented Kalman filter (UKF), could also be applied as long as its error dynamic is known. Applying another filter requires adjusting (6), (7), (16), and (17) according to the filter dynamic.

figure d

Experimental evaluation

In this section, we first introduce the hardware and software setup for data recording, followed by the parameterization of the variables mentioned in the methodology. We designed and summarized three experiments using the dataset, providing their results in a separate section.

Experiment setup

The designed approach is validated in a post-processing environment using the data recorded on a commercial land vehicle in a real-world test campaign. Figure 2 shows the experimental vehicle with the measurement setups for real-time data recording. The lever arm of the antenna position in the IMU body frame \({{\varvec{l}}}_{A}^{b}\) is measured as follows:

$${{\varvec{l}}}_{A}^{b}={\left(\begin{array}{c}\begin{array}{ccc}0& 0& -0.1131 \mathrm{m}\end{array}\end{array}\right)}^{T}$$
(21)

which is applied in (4) and (5) for the lever arm compensation.

Fig. 2
figure 2

Measurement setup equipped on a car: the antenna and IMU were mounted on the roof (left); the rest of the hardware was placed in the trunk (right)

Full size image

Table 1 gives a list of the hardware and the corresponding parametrization. Specifically, the parameterization of the measurement noise (\({\nu }_{\rho }\) and \({\nu }_{d}\)) variance matrix \({\varvec{R}}\) for all GNSS observables is crucial. Breuer et al. (2016) and Konrad et al. (2018) apply the sigma-ε model for this purpose, which calculates the standard deviation for each pseudorange and deltarange as:

$${\sigma }_{\rho }^{2}={C}_{\rho }^{2}\cdot {10}^{-\frac{{\mathrm{C}/\mathrm{N}}_{0}}{10}}, { \sigma }_{d}^{2}={C}_{d}^{2}\cdot {10}^{-\frac{{\mathrm{C}/\mathrm{N}}_{0}}{10}}$$
(22)

where \({\mathrm{C}/\mathrm{N}}_{0}\) is the carrier-to-noise ratio in dB-Hz and provided by the GNSS receiver. \({\mathrm{C}/\mathrm{N}}_{0}\) varies among satellites and with time. \({C}_{\rho }\) and \({C}_{d}\) are tuning parameters defined for the pseudorange and deltarange measurements. The choice of \({C}_{\rho }\) and \({C}_{d}\) is evaluated and discussed in the next section.

Table 1 List of the applied hardware and the specifications. The PPS is used for measurement delay compensation (Konrad et al. 2018)
Full size table

The initial estimation error covariance matrix of the EHF \({{\varvec{P}}}_{0}\) is calculated by the initial standard deviation \({{\varvec{\sigma}}}_{0}\) given in Table 2.

Table 2 Standard deviations of the initial states in the EHF
Full size table

The reduction order parameter q of zonotope is introduced in (A16) and is discussed with experimental results later in this section. Besides, \({n}_{\sigma }\) is set to 3 for the zonotope initialization.

Experiment overview

In this section, the test drive of the experimental evaluation is described, followed by an overview of the designed experiments. Figure 3 shows the bird-eye view of the RTK reference trajectory on the Google-Map-satellite-view layer. The testing area locates in Aachen, Germany, and is a mix of boulevard, semi-urban, sub-urban, and highway environments. The detailed testing area division refers to (Nitsch et al. 2021), where the test drive was carried out in the same area but applied to another measurement setup.

Fig. 3
figure 3

Bird-eye view of the reference trajectory: valid RTK reference position in blue; invalid non-RTK reference position in yellow; unavailable reference position in red, where the positioning solution is estimated by the navigation filter. Map data ©2022 GeoBasis-DE/BKG (©2009), Google

Full size image

The entire test drive takes 1514 s and is divided into three rounds, the overview of which is given in Table 3.

Table 3 Description of the three test rounds
Full size table

Combining Figs. 3, 4, and Table 3, in Round I, the vehicle drives at a speed of up to 50 km/h in an interference-lack environment, where the number of available satellites is stable, and the \({\mathrm{C}/\mathrm{N}}_{0}\) plot indicates that the signal quality is high and stable. Round II and III are much more challenging for satellite navigation, with high-speed tracks up to 70 km/h, and the vehicle drives under bridges, as marked in Fig. 3. The changing number of visible satellites and their unstable \({\mathrm{C}/\mathrm{N}}_{0}\) even caused failed positioning from the reference receiver for a long period of around 500 s.

Fig. 4
figure 4

Receiver data during the test drive: the absolute velocity estimated by the GNSS receiver, the PVT mode of the receiver where only RTK fix and float can be considered as valid reference, the number of available satellites during the drive, and the carrier-to-noise ratio of the tracked GPS and Galileo signals

Full size image

Considering the characteristics of the current dataset, we split the entire test drive into a dataset for parametrization (Round I) and a dataset for validation (Round II and III). And following experiments are designed:

  • Section Sensitivity analysis of estimation error vs. \({C}_{\rho }\) and \({C}_{d}\): Here, we analyze the sensitivity of the state estimation accuracy from the EHF and EKF by applying a grid search upon the parameter set \({C}_{\rho }\) and \({C}_{d}\). This experiment uses the dataset Round I and determines the \({C}_{\rho }\) and \({C}_{d}\) that are applied in the following experiments.

  • Section "Comparison between EHF and EKF regarding state estimation accuracy": Here, we evaluate the state estimation accuracy of EHF and EKF using the datasets Round II and III, with the determined parameters from Round I. The idea is to evaluate the robustness against sub-optimal parameter settings since the applied parameter set is determined in a different environment, namely Round I, rather than its actual operation environment, namely Round II and III, which contains much more signal disturbances.

  • Section "Validation of PL generation using zonotope": Here, we discuss the choice of reduction order q by applying three and nine-dimensional zonotopes and validate the PL estimation by applying the Stanford-ESA Integrity Diagram using the validation dataset Round II and III.

Sensitivity analysis of estimation error vs. \(C_{\rho }\) and \(C_{d}\)

This section analyzes and compares the sensitivity of the state estimation accuracy from EHF and EKF against the parameter set \({C}_{\rho }\) and \({C}_{d}\). A grid search is performed within the parameter range \({C}_{\rho }\in [10, 4000]~\mathrm{ m}/\sqrt{\mathrm{Hz}}\) and \({C}_{\mathrm{d}}\in [0.25, 192]~\mathrm{ m}/\left(\mathrm{s}\cdot \sqrt{\mathrm{Hz}}\right)\). Considering the C/N0 values plotted in Fig. 4, which are within the range of \(\mathrm{C}/\mathrm{N}_0\in [20, 50]\) dB-Hz, the range of the standard deviation of pseudorange and deltarange observables can be estimated by applying the sigma-ε model given in (22) and summarized in Table 4.

Table 4 Standard deviation of the pseudorange and deltarange estimated by the sigma-ε model
Full size table

The grid search results are shown in Fig. 5. Comparing the subfigure top left and right, the minimum average 3D positioning error of both filters is approximately 0.62 m, indicating that the EHF has no advantage over the EKF under optimal parameter settings. However, the positioning accuracy of the EHF is less sensitive to the parameters (varies between 0.62 m to 1 m) than the EKF (varies between 0.62 m to 2 m). Comparing the subfigure bottom left and right, the EHF is slightly less sensitive to the parameters regarding the velocity-estimation accuracy. It should be noted that these results are against the statement in (Simon 2006, pp. 368): ‘the filter (EHF) performance is more sensitive to the design parameters. The reason is that, instead of the numerical example in (Simon 2006), the navigation filter has much larger system uncertainties, such as GNSS signal disturbances. Under poor parametrizations, the unmodeled measurement error dominates the filter’s performance. In this case, the EHF outperforms the EKF due to its filter robustness against system disturbances.

Fig. 5
figure 5

Average position and velocity-estimation accuracy of the EHF and EKF under various \({C}_{\rho }\) and \({C}_{d}\) applying the data from Round I. Each sample data is an individual simulation, and the contour plots are generated with the MATLAB® contourf.m function with default data interpolation. Note that both the x- and y-axes are logarithmic representations

Full size image

As mentioned in the experiment overview, a parameter set of \({C}_{\rho }\) and \({C}_{d}\) shall be chosen and used for the following evaluations. It is noticeable that the minimum values are different in each subfigure in Fig. 5, meaning that there are no optimal parameter settings for both filters. Considering that the evaluation in the next section aims at proving the robustness of the EHF under non-optimal conditions, we chose the parameter set, therefore, for the best benefit of the EKF, applying the following cost function:

$$\text{weighted }3\mathrm{D}\text{ error = }\frac{\text{ mean }3\mathrm{D}\text{ position error }}{max(\text{mean }3\mathrm{D}\text{ position error})}+\frac{\text{ mean }3\mathrm{D}\text{ velocity error }}{max(\text{mean }3\mathrm{D}\text{ velocity error})}$$
(23)

With (23), the 3D position and velocity error under each parameter setting are normalized by the maximum error of all parameter combinations and equally weighted. Applying (23), the parameter set is chosen as \({C}_{\rho }=100~\mathrm{ m}/\sqrt{\mathrm{Hz}}\) and \({C}_{d}=1~\mathrm{ m}/\left(\mathrm{s}\cdot \sqrt{\mathrm{Hz}}\right)\). The corresponding standard-deviation ranges estimated by the sigma-ε model are given in Table 4.

Comparison between EHF and EKF regarding state estimation accuracy

This section evaluates the state estimation accuracy of the EHF and EKF using the validation data from Round II and III, applying the parameter set \({C}_{\rho }=100~ \mathrm{ m}/\sqrt{\mathrm{Hz}}\) and \({C}_{d}=1~\mathrm{ m}/\left(\mathrm{s}\cdot \sqrt{\mathrm{Hz}}\right)\), which is optimized using data from Round I for the EKF. Figure 6 shows the experimental results, and Table 5 shows various metrics of positioning accuracy.

Fig. 6
figure 6

Comparison of the estimation accuracy of the EHF and EKF: horizontal and vertical position error in meter; horizontal and vertical velocity error in meter per second

Full size image
Table 5 Accuracy indicators of horizontal and vertical positioning error of EHF and EKF
Full size table

The first subfigure shows that the EHF has a slight advantage over the EKF regarding horizontal positioning accuracy, the same as in Table 5. However, under extreme disturbances, e.g., at around 500 and 1350 s when the vehicle drives under a bridge and then, directly into environments that lack satellite signals, both the EHF and EKF experience horizontal error up to meters.

Meanwhile, the vertical error comparison in the second subfigure and Table 5 indicates a more accurate estimation from the EHF than from the EKF. Under extreme disturbances, the EHF maintains a vertical error of under 2 m, while the EKF has a vertical error of up to 4 m. The results of the horizontal and vertical position errors show that EHF is more robust against environmental changes and signal disturbances by applying the same parameters.

Noteworthy, applying the EKF, the horizontal estimation error is smaller than the vertical one, which is consistent with expectations: During the drive, the average horizontal and vertical dilution of precision (DOP) is 0.862 and 1.707, respectively. This results from the satellite-constellation limitation that the satellites distribute only above the horizon (Kaplan and Hegarty 2005, pp. 328–332). With the given DOPs, the vertical positioning uncertainty is expected to be twice as large as the one in the horizontal direction if a least square estimator was applied. However, applying the EHF, the horizontal estimation error is surprisingly larger than the vertical one, which further proves the robustness and advantage of EHF against non-optimal operation conditions, i.e., non-optimal satellite constellation.

Finally, the third and fourth subfigures show no significant difference between EHF and EKF regarding the accuracy of velocity estimation, which is similar to the results in Fig. 5. Due to the lack of accurate orientation reference, the accuracy of the orientation estimation is not included.

More experiment results can be found in the supplementary material, where the EHF outweighed the EKF under various parametrizations of GNSS measurement noise, not only in vertical but also in horizontal positioning. Under extreme system disturbances, the EKF estimate diverges from the ground truth, while the EHF estimation error maintains limited, proving the EHF’s theoretical strength.

Validation of PL generation using zonotope

This section first discusses the choice of the reduction order q and its influence on the zonotope estimation. It then validates the correctness of the PL estimation by applying the Stanford-ESA Integrity Diagram. Noteworthy, all PLs generated by zonotopes in this section are 3 \(\sigma\)-boundaries, corresponding to an interval integrity risk of 0.27% by initialization. It should be emphasized that the overall integrity risk shall be much lower, as discussed with (20).

Figure 7 shows the average width of the estimated PL by applying three- and nine-dimensional zonotopes under various reduction order q within the EHF. We conducted the post-processing using MATLAB and Simulink platform on a PC equipped with an Intel® Core (TM) i7-11700 K @ 3.60 GHz CPU. The results in Fig. 7 include the run time of the EHF together with the zonotope computed on a single CPU core.

Fig. 7
figure 7

Computation time and mean value of the generated HPL and VPL for the position estimation using 3D and 9D zonotopes with various reduction order q

Full size image

As discussed in Fig. 9, the smaller q indicates a larger overestimation of the zonotope and a smaller computational load. This is verified by the results in Fig. 7, where the following observations should be stressed:

  • The computational time of the zonotopes increases quasi-proportionally to the reduction order q.

  • The computational time of the zonotopes is not proportional to the dimension of the zonotopes. With the same reduction order \(q=20000\), the run time of the 9D zonotope is only 1.8 times that of the 3D zonotope.

  • Applying high-dimensional zonotopes makes the overestimation effect more severe, especially with small reduction orders. By reduction order \(q=100\), \(300\), and \(500\), the average HPL (9D) is approximately \(8.7\cdot {10}^{27}\), \(1.7\cdot {10}^{4}\), and 137 m, respectively. For better visualization, they are not shown in Fig. 7.

  • With small reduction orders, the estimated HPL is larger than the VPL because zonotope’s overestimation effect dominates the PL generation. The calculation of HPL regards two states (position in north and east direction), which is affected by the overestimation more severely than VPL, which only regards one state (position in down direction). The overestimation effect becomes less dominant with the increasing reduction order, and the measurement update (given as (17)) contributes more to the PL generation.

  • Before \(q=20000\), increasing the reduction order reduces the overestimation effect of zonotopes significantly. After \(q=20000\), the benefit of further increasing the reduction order is not obvious. Moreover, it increases the risk of violating the 10 ms real-time threshold.

Summarizing the results, we can conclude that applying high-dimensional zonotopes requires larger reduction orders, which is more computationally consuming. Therefore, the choice of the zonotope dimension should be prudent and consider the application requirements to avoid wasting computing power.

The performance of the PL estimation is commonly evaluated by applying the Stanford-ESA Integrity Diagram, presented in Fig. 8. The core issue of the PL calculation is to bound the state estimation error, requiring the estimated PL of a state to be larger than the state error. Namely, the sample points shall locate in the top left area in Fig. 8. Further, the estimated PL shall be under a pre-defined limit, namely the alarm limit (AL). Otherwise, the integrity system shall be declared unavailable. Finally, the red area shall be avoided as the highest priority for integrity systems, namely the hazardous operation (also known as hazardously misleading information (HMI)), where the actual state estimation error is beyond the AL, and the estimated PL is under the AL. To conclude, the sample points shall locate in the nominal operation (NO) area, while the estimated PLs should be kept as small as possible.

Fig. 8
figure 8

Stanford-ESA Integrity Diagram with 105,448 epochs. NO = nominal operation; MI = misleading information; HO = hazardous operation; SU = system unavailable. This plot is generated with (Ogier 2022)

Full size image

The results in Fig. 8 are the position PLs generated by applying the three-dimensional zonotope. We cannot evaluate the PLs of velocity qualitatively due to insufficient accuracy and precision of the reference, which we demonstrate in the supplementary material. Further, we cannot evaluate the PLs of the orientation due to the missing reference data. Thus, we include the results of the velocity and orientation PLs generated by the nine-dimensional zonotopes in the supplementary material.

Figure 8 shows that, regardless of the choice of the reduction order q, no misleading information is generated by the zonotope, verifying the correctness of the zonotope-based PL estimation. With this result, no integrity risk is observed with the proposed approach. Further, comparing the generated PL by applying \(q=1000\) and \(q=20000\), the overestimation effect with reduction order \(q=1000\) is notable: The sample points are distributed almost evenly along the y-axes, regardless of the positioning error. This also results in system unavailability of up to 39.084%. By applying \(q=20000\), all samples locate in the nominal operation area for HPL estimation.

Table 6 introduces the essential metrics to evaluate the zonotope-based HPL calculation and compares them with the results of the innovation-based method (Zhu et al. 2018), where an EKF applying the same sigma-ε model is developed. The results indicate that the zonotope-based approach outperforms the innovation-based approach in terms of misleading information without compromising on the size of the generated HPL and algorithm availability.

Table 6 Comparison of the zonotope- and filter-innovation-based PL generation
Full size table

Still, the integrity requirement for safety–critical applications in (EUSPA 2021) is not met with the proposed system, namely HPL under 2 m. Such a requirement cannot be satisfied by simply increasing the reduction order. Instead, further reducing HPL relies on limiting the measurement uncertainty and, therefore, might be satisfied by integrating further GNSS observations, i.e., carrier phase, into the tightly coupled navigation filter and integrity system.

Conclusion

This manuscript presented a novel robust state and protection-level estimation scheme within a tightly coupled navigation system involving the EHF and zonotope. The EHF guarantees a robust state estimation under poor parametrization or system disturbances, which was validated with real-world data from mixing environments (including semi/sub-urban environments) in post-processing. By grid searching the same parameter space of the GNSS measurement noise, the 3D positioning accuracy of the EHF varies between 0.6 m and 1 m, whereas the one from the EKF varies between 0.6 m and 2 m. This result proved that the estimation accuracy of the EHF is less sensitive to the GNSS measurement parametrization and, therefore, has less requirement on the measurement noise modeling. Applying the same GNSS measurement parametrization, the EHF shows higher robustness against real-world disturbances and outperforms the EKF regarding positioning accuracy, especially in the vertical direction. Considering the difficulty of the appropriate parametrization by applying the EKF within a GNSS/INS, the EHF is suggested to be an ideal alternative to the EKF in urban environments, where the quality of GNSS measurements varies continuously due to changing operating environments. The EHF could benefit UAV applications where vertical localization is essential.

Further, a feasibility study was carried out for the zonotope-based PL generation in a high-order nonlinear, tightly coupled GNSS/INS. In theory, applying zonotope releases the PL generation from the statistical assumptions. The experimental results prove that applying a zonotope for PL generation is valid when the reduction order is appropriately set. Compared to the conventional filter-innovation-based approach, applying zonotope decreases the probability of misleading information to zero and slightly reduces the size of generated PL. This approach is computationally intensive when a large reduction order is required. However, the results prove that a real-time implementation is promising. The size of estimated PLs should be further reduced by applying carrier phase measurements to apply zonotope for safety–critical autonomous applications.