Robust state and protection-level estimation within tightly coupled GNSS/INS navigation system

In autonomous applications for mobility and transport, a high-rate and highly accurate vehicle-state estimation is achieved by fusing measurements of global navigation satellite systems (GNSS) and inertial sensors. The state estimation and its protection-level generation often suffer from satellite-signal disturbances in urban environments and subsequent poor parametrization of the satellite observables. Thus, we propose an innovative scheme involving an extended H_infinity filter (EHF) for robust state estimation and zonotope for the protection-level generation. This scheme is shown as part of a tightly coupled navigation system based on an inertial navigation system and aided by the GPS/Galileo dual-constellation satellite navigation system. Specifically, GNSS pseudorange and deltarange observables are utilized. The experimental results of post-processing a real-world dataset show significant advantages of EHF against a conventional extended Kalman filter regarding the navigation accuracy and robustness under various GNSS-measurement parametrizations and environmental circumstances. The zonotope-based protection-level calculation is proven valid, computationally affordable, and feasible for real-time implementations.

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 are compared by applying the wellknown 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.
Additionally, GNSS fault detection and exclusion (FDE), proposed in previous works (Liu et al. 2019;Liu et al. 2021), is performed after the correction of GNSS observables, such that a set of fault-free GNSS measurements is fed into the EHF.

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 is designed: Additionally, the accelerometer bias and gyroscope bias , each of dimension ℝ 3×1 , are part of the state vector, which is necessary for IMU integration. Further, the receiver clock bias and drift are estimated, which are essential for tightly coupled GNSS integration.
As shown in Fig. 1, the state vector and its covariance 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 and the noise shaping matrix , 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 , which is sufficiently described in (Wendel 2009) or (Groves 2013).

Measurement Model
The measurement equation of pseudorange is given as follows: where ̃ is the measured scalar pseudorange of one satellite, and represent the position of a satellite (denoted by ' ') and the antenna in ECEF coordinates displayed in ECEF, respectively. The terms , , , , , 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 Sec. Experiment Setup.
The deltarange observation is calculated as: where ̃ is the measured scalar relative velocity between the antenna and satellite, known as the deltarange. The vector is the direction from the antenna to a satellite. The terms and represent the velocity of the satellite and antenna with respect to the navigation frame, respectively. The terms and 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 and to the required states and in the IMU body-frame, shown as: where and denote the direction cosine matrix for the vector rotation among coordinate frames, derived from the estimated antenna position and the estimated attitude (Groves 2013), respectively. The transformation uses the lever arm of the antenna position in the IMU body-frame, measured and given in Sec. Experiment Setup, and considers the translation velocity of the antenna caused by its rotation , derived from the IMU measurement and estimated IMU bias (Wendel 2009).
(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 .
Analog to the filter state transition, the estimation of the state error bound [ ] also contains a-priori propagation and a-posterior correction. The error dynamic of the EHF is given as follows: where − and + represent the a-priori and a-posterior estimation error. ̂−, ̂+, and are filter-relevant variables and defined in (A4)~(A8).
Assume that the initial estimation error 0 , the process noise and the measurement noise are zero-mean and bounded by corresponding zonotopes: where 0 , and represent the generation matrix of corresponding zonotopes, defined in (A12). 0 shall be initialized large enough, considering the large system uncertainties in the filter initialization stage. and are calculated as diagonal matrices with their diagonal elements equal to times the standard deviation of process noise and measurement noise , respectively. is a pre-defined parameter corresponding to the confidence level of the resulting zonotope.
Noteworthy, for the calculation of and , 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.
(9) and (10) result in the following zonotope propagation and update steps: Each zonotope estimation carries out a zonotope reduction, explained with (A16) and denoted as 'ℛ'. The dimension of the resulting zonotope is specified by a userdefined reduction order ( ≤ ≤ ), namely ℛ( ) ∈ ℝ × . Without applying the zonotope reduction, the dimension of the generation matrices − and + increases without limit along the operation time, which is not affordable for computation units.
In the EHF, the state vector contains 18 states, including the quaternion ∈ ℝ 4×1 . By the estimation of the state error covariance , the error of the orientation vector ℝ 3×1 is considered (Wendel 2009), which makes ∈ ℝ 17×17 . Therefore, the error space of the EHF is 17-dimensional. It results in the generation matrix for the EHF ∈ ℝ 17× . 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 where is the dimension of the desired zonotope. A selection matrix ∈ ℝ ×17 can be applied to the generation matrix calculation to achieve the state selection. Γ denotes the element of the i th column and j th raw in and is given as: The calculation of the zonotope < , , > to bound the selected state errors is given as follows: , 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 , ∈ ℝ 9× . The zonotope could be further reduced to three-dimensional , ∈ ℝ 3× 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 ∈ ℝ 3×17 and ∈ ℝ 9×17 : 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: where is the unknown ground truth of the state estimate ˆ. denotes the probability of integrity risk. is the vector calculated for all desired states and corresponds to the dimension of .
Assuming that (8) is satisfied, (11) indicates that the estimation error must be within the zonotope given as < , > 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 < , , > 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 < , 0 >, < , >, and < , > can be quantified with the pre-defined parameter . 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 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.

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. Fig. 3 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 is measured as: which is applied in (4) and (5) for the lever arm compensation.  where C/N 0 is the carrier-to-noise ratio in dB-Hz and provided by the GNSS receiver. C/N 0 varies among satellites and with time. and are tuning parameters defined for the pseudorange and deltarange measurements. The choice of and is evaluated and discussed in the next section.
The initial estimation error covariance matrix of the EHF 0 is calculated by the initial standard deviation 0 given in Table 2. The reduction order parameter q of zonotope is introduced in (A16) and is discussed with experimental results later in this section. Besides, 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. Fig. 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. The entire test drive takes 1514 seconds and is divided into three rounds, the overview of which is given in Table 3.  Fig. 4, which are within the range of C/N0 ∈ [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.  (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. As mentioned in the experiment overview, a parameter set of and 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: With (23) 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 = 100 m/√Hz and = 1 m/(s • √Hz), which is optimized using data from Round I for the EKF. Fig. 6 shows the experimental results, and Table 5 shows various metrics of positioning accuracy. 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 seconds 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 smaller 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.  (20).  As discussed in Fig. A1, 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 = 20000, the run time of the 9D zonotope is only 1.8 times that of the 3D zonotope.
• 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 = 20000, increasing the reduction order reduces the overestimation effect of zonotopes significantly. After = 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.   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 innovationbased approach in terms of misleading information without compromising on the size of the generated HPL and algorithm availability.

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.
the German Bundestag. Special thanks go to colleagues Jiaying Lin and David Benz for proofreading the article.

Data Availability
The datasets used and generated in this research are available from the corresponding author upon reasonable request.

Appendix: H∞ Filtering and Zonotope Theory
For the brevity of the main text, we present the basic theory of the EKF and zonotope in this appendix.

Theory of Extended H∞ Filter
Considering a nonlinear time-discrete system given as follows: where and −1 are the state vectors for time instance and − 1, respectively.
is the measurement vector. and are the nonlinear time-invariant state transition and measurement functions, respectively. and are the process and measurement noise vectors, respectively. denotes the shaping matrix, which maps the process noise into the state vector . and are assumed to satisfy a white Gaussian distribution, whose variance matrices are and , respectively. denotes the desired estimation vector at the time instance , which is defined by a user-designed matrix (full rank). Concerning the navigation solution, all states in shall be estimated such that is defined as a unit matrix.
The complete H∞ filtering computation steps are rigorously derived in (Simon 2006) and summarized in (Loo et al. 2019) as: where ̂− , ̂− 1 + , ̂− , and ̂+ denote the a-priori and a-posteriori state and covariance estimate, respectively. −1 and are the Jacobian matrix of and , linearized at the operating points, respectively. is a user-defined weighting matrix, specified as a unit matrix in current work, which weights the elements of in the cost function given in (A9). is the filter gain matrix. It should be noted that the calculation of involves a parameter , discussed later in this section.
The purpose of the optimal H∞ filtering is to choose the suitable estimation strategy that minimizes the following cost function: where 0 , ̂0 and 0 represent the initial state vector, its estimation, and the covariance of the initialization error, respectively. ̂ is the estimate of . and is assumed to improve the filter robustness against worst-case situations (Simon 2006, p. 343).
However, the cost function cannot be directly minimized (Simon 2006, p. 344).
Thus, a performance bound −1 is introduced as a constraint for the optimization: Solving the constrained optimization problem results in the estimation strategy (A6)~(A8). Meanwhile, the constrained optimization problem is only solvable when the following inequality condition is satisfied (Simon 2006, pp. 351-352): where '≻' denotes that the resulting matrix on the left-hand side is positive-definite.
It can be observed that the larger is , the more difficult that (A11) can be satisfied.
Thus, shall not be chosen too large. The choice in the vicinity of infinity might cause the divergence of the EHF (Hassibi et al. 1996). On the other hand, Simon (2006) has described as a measure of filter robustness, i.e. the larger , the smaller estimation error under worst-case situations, and therefore, the stronger robustness of the filter. Thus, the choice of is crucial and decides the EHF performance. In practice, could be chosen conservatively by experience or estimated by solving the linear matrix inequality (LMI) problem described by (A11). We apply an LMI solver from MATLAB ® to calculate .

Theory of Zonotope
Zonotope is a type of centrally symmetric convex polytope. Zhang et al. (2020) have applied zonotope to present the bounding area of the estimated states within a filter. This section introduces the definition and necessary background knowledge of zonotope. where vector ∈ ℝ is the center of ℤ. ∈ ℝ × is called the generation matrix of zonotope ℤ, which defines the shape and direction of ℤ. Each column of = [ , . . . , ] denotes a translation direction and amplitude in Euclidean space. For a simple expression, we use ℤ =< , > to denote a zonotope. Fig. A1 presents an example of a two-dimensional and six-order zonotope as the yellow area.
In Fig. A1, an example of reducing the two-dimensional zonotope (ℤ =< , > ⊂ ℝ 2 ) with six-order ( ∈ ℝ 2×6 ) is illustrated. ℤ is reduced to ℤ 1 and ℤ 2 with reduction order 1 = 4 and 2 = 3 , respectively. A smaller reduction order results in a larger overestimation but reduces the computational load. The reduction order is allowed to be freely chosen (Combastel 2003), compromising between the exactness (with larger ) and the domain complexity (with smaller ) of the resulting zonotope.