A Dual w-Test Based Quality Control Algorithm for Integrated IMU/GNSS Navigation in Urban Areas

Abstract

Integration of the Global Navigation Satellite System (GNSS), with Inertial Measurement Unit (IMU) sensors to improve navigation performance, is widely used in many land-based applications. However, further application, especially in urban areas, is limited by the quality (due mainly to multipath effects) and availability of GNSS measurements, with a significant impact on performance, especially from low grade integration. To maximize the potential of GNSS measurements, this paper proposes a dual w-test-based quality control algorithm for integrated IMU/GNSS navigation in urban areas. Quality control is achieved through fault detection and exclusion (FDE) with the capability to detect simultaneous multiple faults in measurements from different satellites. The remaining fault-free GNSS measurements are fused with IMU sensor measurements to obtain the final improved state solution. The effectiveness of the algorithm is validated in a deep urban field test. Compared to the cases without fault exclusion, the results show improvements of about 24% and 30% in horizontal and vertical positioning components, respectively.

1. Introduction

The emerging mission-critical applications in urban areas are placing more stringent requirements on the underpinning positioning, navigation, and timing (PNT) systems [1]. Due to complementary characteristics, GNSS and Inertial Measurement Unit (IMU) sensors are commonly used in an integrated architecture to support location-based services. However, in urban areas, GNSS signals are susceptible to attenuation and blockage in the built environment, resulting in multipath effects and non-line of sight (NLOS) reception. The satellite faults, defined in this paper, describe corresponding measurements that have acceptable errors, irrespective of the source and type of failure. These errors in the measurements will affect the accuracy and reliability of positioning from integrated IMU/GNSS systems. Therefore, it is particularly important to develop an effective fault detection scheme that can be applied to GNSS measurements so as to ensure quality control of integrated IMU/GNSS systems.

Fault Detection and Exclusion (FDE)-based GNSS measurements quality control has been investigated for many years. The basic FDE methods include: (1) range and position comparison [2]; (2) minimum least squares residuals [3]; (3) parity space [4]; (4) maximum slope (MS) [5]. The four methods have been shown to be largely equivalent.

The performance of FDE algorithms is related to GNSS signal quality and the number of visible satellites. With the increase in constellations beyond GPS, there are more visible satellites and better signal design, greatly improving positioning quality, and promoting the development of FDE algorithms. Some new FDE algorithms have appeared, such as: GPS Integrity Channel (GIC), which is a hybrid between the GIC approach and the maximum solution separation RAIM technique [6]; Novel Integrity Optimized RAIM [7]; Optimally Weighted Average Solution [8]. Given that the probability of multiple faults in a single constellation is relatively small, the above FDE algorithms assume a single fault at a time.

In medium to high density built environments coupled with the increase in the number of constellations, the probability of simultaneous multiple faults increases. Therefore, increasing research effort is dedicated to developing algorithms for simultaneous multiple FDE. These methods include the use of statistics, calculated based on the w-test, to detect and identify outlier faults [9]. A theoretical analysis of the principle of double satellites faults in 2009, as well as their successful elimination through experiments, is presented in [10]. The Solution Separation (SS) algorithm was also applied to Advanced RAIM (ARAIM) research [11]. A point to note is that, for 4-D positioning and geometry permitting, there must be at least five visible satellites for fault detection and at least six visible satellites for fault exclusion in a single constellation. When the number of satellites is insufficient, these FDE algorithms are unavailable, thus affecting the quality of GNSS positioning with potential safety risks.

To address the problem of GNSS measurement quality, additional sensors are also used to aid GNSS FDE by considering the various error characteristics of each sensor [12].

Comparison of FDE performance, based on loosely-coupled and tightly-coupled IMU/GPS integration modes, is also analyzed in some literature [13,14]. A multiple fault detection and elimination algorithm, based on pseudorange comparison, is proposed and used for vehicle GNSS/IMU integrated navigation and positioning [15], but it needs initial database generation. In real situations, multipath effects and poor user-satellite geometry result in excessive positioning errors in urban areas, and the methods above cannot verify the correctness or reliability of the FDE algorithms. In addition, the a priori parameters of the measurement covariance matrix cannot be determined in these urban areas. This increases the probability of incorrect fault detection resulting in excessive final positioning errors. A series of adaptive Kalman filters (AKF) have been developed to overcome the limitation of using a priori statistics to model errors that have time-varying characteristics [16,17,18]. The adaptive indicators may take on a range of roles, including an adjustment of the covariance matrix of the state estimation vector, the covariance matrix of the process vector, and the covariance matrix of measurement vector [19,20,21]. None of the adaptive indicators in the above fusion methods, however, have been adjusted specifically for the errors caused by multipath signals and NLOS that are common in urban areas.

In recent years, with the continuous emergence of multi-sensors, the integrated navigation system of multi-source fusion has also ushered in a vigorous development. Altimeter, wheel odometer, magnetometer, etc., improve the accuracy and reliability of navigation information by providing additional information such as position, speed, and altitude to the GNSS/IMU integration system. From the perspective of technology integration, the research on the integration of GNSS, INS, and emerging visual navigation technology is extremely hot. Li developed a semi-tightly coupled GNSS PPP/S-VINS integration framework for better navigation performance in urban environments [22]. On this basis, Li further studied GNSS/LiDAR/INS tightly coupled integrated navigation [23]. However, the above method is in the theoretical research stage, and the high cost of the sensor is not conducive to popularization.

Another idea for quality control is to assign appropriate weights to the GNSS measurements to mitigate the effects of multipath/NLOS signals. The commonly used method is to determine weight based on the quality of GNSS signals. This usually involves the use of one or more characteristics of GNSS signals (e.g., satellite elevation angle, C/N₀, or a combination of the two) to assign corresponding weights to GNSS measurements. Other weighting-based quality control methods include Huber [24], Bifactor reduction model [25], Robust estimation based on M-estimation principle [26], Robust Bayesian estimation [27], and Danish [28]. However, application of appropriate weighting, in different scenarios in urban environments, is difficult. Given the limitations of the state-of-the-art methods above, this paper proposes a dual w-test-based quality control algorithm for integrated IMU/GNSS navigation in urban environments. The contributions are summarized below.

(1) A dual w-test is proposed, which achieves multiple fault detection from the observation domain, thus solving the problem of false alarms in the traditional w-test.

(2) A range detection is proposed to detect the subsets generated after dual w-test, and a scoring strategy is proposed to select the optimal subset. Starting from the location domain, the proposed algorithm is able to reduce the miss detection rate and, therefore, ensure the quality of the output position.

2. Algorithm Design

The proposed tightly-coupled algorithm is illustrated in Figure 1 and comprises two parts. In the first part, a dual w-test-based FDE model is designed for multiple failure detection in urban areas. In the second part, a scoring strategy is used to exclude faulty measurements. The remaining satellites are then fused with IMU sensor measurements to compute the final state.

2.1. Dual w-Test

2.1.1. Traditional w-Test

Due to the non-linear relationship between the GNSS pseudorange observation and state variables [29], the linearized pseudorange observation equation can be written as (1).

$$Y=HX+\epsilon$$

(1)

Here, $Y$ is the difference between the observed pseudorange and computed pseudorange from the initial state, $H$ is the measurement matrix, $X$ is the user’s state vector, and $\epsilon$ is the observation error vector. The weighted least squares solution for the state vector $X$ is (2).

$$X_{\mathrm{WLS}}={\left(H^{T}WH\right)}^{-1}{H}^{T}WY$$

(2)

where, $W$ is the weighting matrix. With $W={(cov(\epsilon ))}^{-1}$, based on Equation (2), the residual vector $r$ is derived as:

$$r=Y-H{X}_{WLS}=\left(I-H{\left(H^{T}WH\right)}^{-1}{H}^{T}W\right)\epsilon =S\epsilon$$

(3)

After obtaining the residual vector $r$, the sum of the squares of the residual or error (SSE) vector is used as the statistics for GNSS fault detection, which is defined as (4)

$$SSE=r^{T}Wr$$

(4)

Based on weighted least squares residuals, GNSS pseudorange measurements with significant errors are detected and eliminated by overall and local inspection methods. The overall test assumes that when observations do not contain gross errors, the observation errors follow the Gaussian distribution. Hence, the statistic SSE follows the chi-square distribution with degrees of freedom (n − m), where n is the total number of satellites observed, and m is the dimension of the state. When the test statistic exceeds the global threshold, there is at least one faulty satellite. The global test threshold $T_G$ is:

$$T_G={\chi }_{1-P_{FA},\left(m-n\right)}^2$$

(5)

where $P_{FA}$ is the probability of false alarm, which is selected according to specific application scenarios, and ${\chi }^2$ denotes the probability density of the chi-square distribution. When the statistic exceeds the global threshold, it is necessary to find the failing measurement or gross error in observations, using the traditional w-test. The test normalizes the residual as a new statistic. The specific expression is (6):

$$w_i=\frac{e_i^{T}r}{\sqrt{e_i^{T}S{e}_i}},i=1:m$$

(6)

where $e_i$ is the unit vector whose i-th element is 1. When the i-th observation has no error, the variance of the corresponding observation noise ${\epsilon }_i$ is ${\sigma }^2$, with $w_i$ following the normal distribution $N(0,{\sigma }^2)$. ${\left|w_i\right|}_{\max }$ is then compared with the w-test threshold $T_L$. If ${\left|w_i\right|}_{\max }$ exceeds the threshold, it is considered that the corresponding observation contains gross error. Then, the traditional w-test eliminates the corresponding satellite. The expression of the w-test threshold is:

$$T_L=N_{1-P_{FA}/2}\left(0,{\sigma }^2\right)$$

(7)

The traditional w-test only identifies one faulty satellite at a time, and the ${\left|w_i\right|}_{\max }$ corresponding satellite is eliminated. At the same time, in order to confirm whether there are any faulty satellites in the remaining satellites, all the remaining satellites after w-test are regarded as a new corpus again, and a new round of fault detection is performed. Therefore, the w-test method is suitable for the case of highly redundant observation data, and it is assumed that only one failure occurs at a time. In urban environments, however, this condition may not be met. Therefore, this paper adopts $3‑\sigma$ and $1‑\sigma$ w-test double w-test, as shown in the following subsection.

2.1.2. $3‑\sigma$ and $1‑\sigma$ Dual w-Test

Different from the traditional w-test, this paper firstly adopts $3‑\sigma$ w-test. The first $3‑\sigma$ w-test is to prevent the observation noise variance from being too small, as well as too strict, for the corresponding w-test threshold. At the same time, in the $3‑\sigma$ w-test, the method of excluding satellites is not to eliminate the ${\left|w_i\right|}_{\max }$ corresponding satellite, but it is to eliminate the corresponding satellite with the largest absolute value of the predicted pseudorange residual when the ${\left|w_i\right|}_{\max }$ exceeds the threshold. The predicted pseudorange residual is calculated as (8):

$$\Delta \rho ={\rho }_{IMU}-{\rho }_{GNSS}=r_{IMU}+c\left(d{t}_R-d{t}^S\right)+I_{\rho }+T_{\rho }-{\rho }_{GNSS}$$

(8)

where, ${\rho }_{GNSS}$ is the observed pseudorange, ${\rho }_{IMU}$ is the pseudorange predicted by IMU, $r_{IMU}$ is the geometric range between the observed satellite and the user position estimated by IMU. $d{t}_R$ and $d{t}^S$ are the receiver and satellite clock errors, respectively, $I_{\rho }$ and $T_{\rho }$ are tropospheric and ionospheric corrections, respectively. The $r_{IMU}$ can be calculated as (9):

$$r_{IMU}=\sqrt{{\left(X_k^G-X_k^{IMU}\right)}^2+{\left(Y_k^G-Y_k^{IMU}\right)}^2+{\left(Z_k^G-Z_k^{IMU}\right)}^2}$$

(9)

$\left(X_k^G,Y_k^G,Z_k^G\right)$ is the satellite position at epoch $k$, $\left(X_k^{IMU},Y_k^{IMU},Z_k^{IMU}\right)$ is user positions estimated using IMU data at epoch $k$. However, due to the complexity of urban environments, it is impossible to ensure correct detection using the $3‑\sigma$ w-test. Therefore, the positions calculated before and after each $3‑\sigma$ w-test are saved until either no faulty satellite measurements are detected or the number of remaining observed satellites is insufficient. Then, in order to ensure that multiple faults can be detected, this paper takes each subset obtained after the $3‑\sigma$ w-test, removing a satellite each time, and performing the $1‑\sigma$ w-test on $C_m^1$ each subset. The results can be one of four cases:

• The universal set and all subsets pass the $1‑\sigma$ w-test.
• The universal set and some subsets pass the $1‑\sigma$ w-test.
• The universal set does not pass the $1‑\sigma$ w-test, and only one of the subsets passes the $1‑\sigma$ w-test.
• The universal set does not pass the $1‑\sigma$ w-test, with more than one subset passing the $1‑\sigma$ w-test.

The fault conditions at a given epoch can be determined by considering the test results in these four cases. In case 1, we consider that there is no faulty satellite at this epoch, as the universal set and all subsets have passed the w-test. In case 2, the high correlation of each satellite will result in the universal set passing the test, while the low correlation of the faulty satellite in the subset, after one satellite exclusion, can result in the subset not passing the test. Therefore, in this case, we consider that there are multiple faults. In case 3, as a single satellite fault can lead to the universal set not passing the w-test, the subset can only pass the w-test in the case that the faulty satellite is excluded. Therefore, a single fault case is considered in this case. In case 4, faulty satellites in the universal set and subsets can lead to the failure to pass the w-test for a part or all of the subsets. Therefore, the existence of multiple faults is considered in this case. Satellite selection is then made according to the fault conditions. In case 1, all of the satellites at this epoch are selected for a further GNSS/IMU integration. In case 3, the satellites in the subset, which passed the w-test, are selected for further fusion. Considering cases 2 and 4 with multiple faults, the $C_m^2$ subsets are further generated, which are then subjected to range detection. The range is calculated by the difference between the predicted position estimated by the IMU data and the position calculated by the selected subset in the proposed algorithm. The expression for range detection is:

$$\left[\begin{array}{c}e\\ n\\ u\end{array}\right]=\left[\begin{array}{ccc}-\sin {\lambda }_0& \cos {\lambda }_0& 0\\ -\sin {\phi }_0\cos {\lambda }_0& -\sin {\phi }_0\sin {\lambda }_0& \cos {\phi }_0\\ \cos {\phi }_0\cos {\lambda }_0& \cos {\phi }_{0}sin{\lambda }_0& \sin {\phi }_0\end{array}\right]\left[\begin{array}{c}{x}_s-x_0\\ y_s-y_0\\ z_s-z_0\end{array}\right]$$

(10)

where ${\lambda }_0$ and ${\phi }_0$ are, respectively, the latitude and longitude corresponding to the predicted position. $x_s,y_s,z_s$ and $x_0,y_0,z_0$, respectively, are the coordinates of the calculated position and the predicted position in the WGS-84 coordinate system. Then,$\left|e\right|$,$\left|n\right|$, and $\left|u\right|$ are compared with the range threshold. Here, the threshold of the range value is set as 17 m, as the city speed limit is around 60 km/h (i.e., 17 m/s). Only the subsets that pass the range detection test are used further for the optimal subset selection.

2.2. Scoring Strategy Based Optimal Subset Selection

After range detection, the subsets that pass the test are selected. The optimal subset within these selected subsets is chosen, and the corresponding measurements in the optimal subset are used to integrate with the IMU data to calculate position. The strategy uses a scoring mechanism to subtract the positions calculated using the selected subsets from the predicted position at the current epoch. The predicted position can be obtained from that of the previous epoch combined with inertial navigation information. The difference in position is then scored according to the following formula, based on a weighting method, in which the smaller the $JointCost$ the higher the score. Finally, the satellites corresponding to the position difference with the highest score are selected to be combined with the inertial navigation. The $JointCost$ is calculated as:

$$JointCost=\frac{\mathit{C}\mathit{o}\mathit{s}\mathit{t}\left(1\right)-Cost1min}{Cost1max-Cost1min}+\frac{\mathit{C}\mathit{o}\mathit{s}\mathit{t}\left(2\right)-Cost2min}{Cost2max-Cost2min}+\frac{\mathit{C}\mathit{o}\mathit{s}\mathit{t}\left(3\right)-Cost3min}{Cost3max-Cost3min}$$

(11)

Here, $Cost1max,Cost2max,Cost3max$ are the maximum values of longitude, latitude, and height difference among all the position differences. $Cost1min,Cost2min$, and $Cost3min$ are the minimum values of longitude, latitude, and height difference among all the position differences. $\mathit{C}\mathit{o}\mathit{s}\mathit{t}(1),\mathit{C}\mathit{o}\mathit{s}\mathit{t}(2),\mathit{C}\mathit{o}\mathit{s}\mathit{t}(3)$ are all the longitude, latitude, and height difference among all the position differences.

2.3. IMU/GNSS Integration

In this section, an Extended Kalman Filter (EKF), based on linearization of nonlinear models, is used as the data fusion algorithm [30]. The state vector for the EKF is:

$$\mathit{X}=\left[\begin{array}{ccccccc}{(\delta {\mathit{r}}_{INS}^e)}^T& {(\delta {\mathit{v}}_{INS}^e)}^T& {({\mathit{\varphi }}_{INS}^e)}^T& {\mathit{b}}_g{}^T& {\mathit{b}}_a{}^T& {\mathit{s}}_g{}^T& \begin{array}{cc}{\mathit{s}}_a{}^T& \begin{array}{cc}{\mathit{t}}_{GPS}& \begin{array}{ccc}\mathit{\delta }{\mathit{t}}_{GPS}& {\mathit{t}}_{BDS}& \mathit{\delta }{\mathit{t}}_{BDS}\end{array}\end{array}\end{array}\end{array}\right]$$

(12)

where, $\delta {\mathit{r}}_{INS}^e$, $\delta {\mathit{v}}_{INS}^e$, and ${\mathit{\varphi }}_{INS}^e$ are the three-axis error vectors of IMU position, velocity, altitude in the ECEF framework $e$; ${\mathit{b}}_g$, ${\mathit{b}}_a$, ${\mathit{s}}_g$, and ${\mathit{s}}_a$ are the three-axis acceleration and gyroscope bias and scale factor error; ${\mathit{t}}_{GPS}$ and $\mathit{\delta }{\mathit{t}}_{GPS}$ are the receiver clock error and drift rate of GPS satellite; ${\mathit{t}}_{BDS}$ and $\mathit{\delta }{\mathit{t}}_{BDS}$ are the clock error and drift rate of Beidou satellite. The system model is then formed as a first-order state equation in (13):

$$\dot{\mathit{X}}=\mathit{F}\mathit{X}+\mathit{G}\mathit{W}$$

(13)

where $\dot{\mathit{X}}$ is the first derivative of $\mathit{X}$. $\mathit{F}$ is the dynamic transition matrix, $\mathit{G}$ is the noise driven matrix, and $\mathit{W}$ is the system noise. The measurement model is given by:

$$\mathit{Z}=\mathit{H}\mathit{X}+\mathit{V}$$

(14)

where $\mathit{Z}$ is the measurements vector, $\mathit{H}$ is the measurement mapping matrix, and $\mathit{V}$ represents the measurement noise. In this paper, if the number of visible satellites is $n$, the pseudorange error and the Doppler measurement error are used to form measurement vector $\mathit{Z}$ as:

$$\mathit{Z}={\left[\begin{array}{c}{\rho }_{1,GPS}^{IMU}-{\rho }_1^{GPS}\\ \begin{array}{c}⋮\\ {\rho }_{l,GPS}^{IMU}-{\rho }_l^{GPS}\\ {\rho }_{1,BDS}^{IMU}-{\rho }_1^{BDS}\end{array}\\ \begin{array}{c}⋮\\ {\rho }_{m,BDS}^{IMU}-{\rho }_m^{BDS}\end{array}\\ f_{1,GPS}^{IMU}-f_1^{GPS}\\ ⋮\\ \begin{array}{c}{f}_{l,GPS}^{IMU}-f_l^{GPS}\\ f_{1,BDS}^{IMU}-f_1^{BDS}\\ ⋮\\ f_{m,BDS}^{IMU}-f_m^{BDS}\end{array}\end{array}\right]}_{2n\times 1}$$

(15)

where ${\rho }_{GNSS}^{IMU}$ and $f_{GNSS}^{IMU}$ denote IMU-derived GNSS pseudorange and Doppler measurements respectively. Based on the derivations in [30], ${\rho }^{GNSS}$ and $f^{GNSS}$ refer to pseudorange and Doppler measurements decoded from GNSS observation data, respectively. $l$ and $m$ refer to the number of GPS and BDS visible satellites. After discretization of (13) and (14), the discrete form of the Kalman filtering procedure can be split into two stages, as follows:

Prediction stage:

$${\widehat{\mathit{X}}}_{k,k-1}={\mathbf{\Phi }}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}$$

(16)

$${\mathit{P}}_{k,k-1}={\mathbf{\Phi }}_{k,k-1}{\mathit{P}}_{k-1}{\mathbf{\Phi }}_{k,k-1}^T+{\mathit{Q}}_{k-1}$$

(17)

Update stage:

$${\mathit{K}}_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}$$

(18)

$${\widehat{\mathit{X}}}_k={\widehat{\mathit{X}}}_{k,k-1}+{\mathit{K}}_k({\mathit{Z}}_k-{\mathit{H}}_k{\widehat{\mathit{X}}}_{k,k-1})$$

(19)

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

(20)

where,${\widehat{\mathit{X}}}_k$ is the system state vector estimates at time epoch $k$; ${\mathbf{\Phi }}_k$ is the system transition matrix at time epoch $k$; ${\mathit{P}}_k$ is the error covariance matrix at time epoch $k$; ${\mathit{Q}}_k$ is the system noise covariance matrix at time epoch $k$; ${\mathit{R}}_k$ is the measurement noise covariance matrix at time epoch $k$; ${\mathit{H}}_k$ is the measurement matrix at time epoch $k$; ${\mathit{K}}_k$ is the Kalman gain matrix at time epoch $k$; ${\Theta }_{k,k-1}$ is the matrix/vector $\Theta$ propagation from time epoch $k-1$ to $k$.

Table 1. The parameters used for the EKF.

Parameter	Initial Value
$\mathit{Q}$	$\mathit{d}\mathit{i}\mathit{a}\mathit{g}\left(\begin{array}{cccccccc}{\mathit{I}}_{3\times 3}\ast {0.0004}^2& {\mathit{I}}_{3\times 3}\ast {0.0005}^2& {\mathit{I}}_{3\times 3}\ast {0.000035}^2& {\mathit{I}}_{3\times 3}\ast {0.00000032}^2& {\mathit{I}}_{3\times 3}\ast {0.0001}^2& {\mathit{I}}_{3\times 3}\ast {0.00001}^2& {\mathit{I}}_{3\times 3}\ast {0.00001}^2& \begin{array}{cccc}{0.001}^2& {0.002}^2& {0.001}^2& {0.002}^2\end{array}\end{array}\right)$
$\mathit{R}$	${\mathit{I}}_{\mathit{n}\times \mathit{n}}\ast {1.5}^2$
${\mathit{P}}_0$	$\mathit{d}\mathit{i}\mathit{a}\mathit{g}\left(\begin{array}{cccccccc}{\mathit{I}}_{3\times 3}\ast {0.025}^2& {\mathit{I}}_{3\times 3}\ast {0.075}^2& {\mathit{I}}_{3\times 3}\ast {0.000035}^2& {\mathit{I}}_{3\times 3}\ast {0.0000097}^2& {\mathit{I}}_{3\times 3}\ast {0.003}^2& {\mathit{I}}_{3\times 3}\ast {0.0025}^2& {\mathit{I}}_{3\times 3}\ast {0.003}^2& \begin{array}{cccc}{0.02}^2& {0.03}^2& {0.02}^2& {0.03}^2\end{array}\end{array}\right)$

has illustrated the parameters and their value or initial value used for the EKF. The setting of the system noise covariance matrix $\mathit{Q}$ is based on experience. The $\mathit{d}\mathit{i}\mathit{a}\mathit{g}$ means that the matrix is a diagonal matrix and the values in the bracket are the diagonal elements. The initial value of error covariance matrix of the state vector $\mathit{P}$, noted as ${\mathit{P}}_0$, is calculated by the historical data collected from the IMU and GNSS receiver. The covariance matrix of the measurement noise $\mathit{R}$ is set based on the statistical data collected from GNSS receiver.

If positions calculated by all subsets do not pass the range detection test or the number of satellites cannot meet the condition of the w-test, then all the satellite measurements and the inertial navigation output information are fused through the robust algorithm. The robust algorithm introduces a fault detection factor $D$ to scale $\mathit{R}$. $D$ is given as:

$$D_{ii}=\{\begin{array}{cc}1,& \left|{\tilde{\Re }}_{k,i}\right|\le Tm\\ \frac{\left|{\tilde{\Re }}_{k,i}\right|}{Tm},& \left|{\tilde{\Re }}_{k,i}\right|>\mathrm{Tm}\end{array}$$

(21)

$${\tilde{\Re }}_{k,i}=\frac{{\Re }_{k,i}}{\sqrt{{ℂ}_{k,ii}}}$$

(22)

${\Re }_k={\mathit{Z}}_k-{\mathit{H}}_k{\widehat{\mathit{X}}}_{k,k-1}$ is the innovation sequence, which exhibits a white Gaussian sequence of mean zero and covariance ${ℂ}_k$ where ${ℂ}_k={\mathit{H}}_k{\mathit{P}}_{k,k-1}{\mathit{H}}_k{}^T+{\mathit{R}}_k$. $Tm$ is a constant value, which is valued according to the specific scenario. Then, the elements in $\mathit{R}$ are given as:

$${\overline{\mathit{R}}}_{k,ii}=D_{ii}\cdot {\mathit{R}}_{k,ii}$$

(23)

3. Test and Validation

3.1. Simulation

Faults are simulated and added to data from UAV flight tests to test the proposed quality control algorithm. The UAV flight data were collected in Nantou City, Taiwan, shown in Figure 2. The UAV used in the test is AXH-E230 from AVIX Technology (Toronto, ON, Canada), and it was flown semi-automatically with a smart power control module to perform autonomous intelligent navigation flight mission. The onboard equipment setup included: (1) a dual-frequency GNSS receiver, Trimble BD 982 (Sunnyvale, CA, USA), with a sampling rate of 10 Hz for the raw pseudorange measurements collection; (2) a STIM-300 IMU (Sensonor, Horten, Norway), with a sampling rate of 100 Hz for UAV acceleration and angular rate collection; (3) an on-board VLP-16 Velodyne Lidar (San Jose, CA, USA) to provide centimeter-level positioning accuracy for the reference trajectory generation in the experiment. The speed of UAV was less than 10 m/s during the flight, and the height was about 60 m AGL (with the ground elevation around 120 m). The fault scenarios in

Table 2. The defined scenarios.

Scenarios	Time Interval of Faults (s)	Error Sources
1	30	10 m, 30 m step errors added to the pseudoranges of two satellites
2	30	10 m, 50 m step errors added to the pseudoranges of two satellites
3	30	30 m, 30 m step errors added to the pseudoranges of two satellites
4	30	30 m, 50 m step errors added to the pseudoranges of two satellites

were specified in order to compare the proposed algorithm with the traditional IMU/GNSS tightly-coupled (TC) without fault exclusion, the TC with traditional w-test quality control (FDE TC), and the TC with Robust filter (AKF TC) in [31].

In the different scenarios above, for each selected satellite, an error of 10 m, 30 m, or 50 m was injected into the pseudo-range observation of the satellite during the corresponding fault duration. Based on the derivations in [32], UAV flight in the urban environment is subjected to multipath interference to produce similar errors, with error magnitudes less than 10 m having little impact on the satellite navigation and positioning results, and is hence ignored as constituting failure. At the same time, considering the characteristics of UAV in urban low-altitude areas, fault duration is selected as 30 s. In order to verify the validity of the algorithm, in terms of accuracy, this paper uses the Root Mean Square Error (RMSE) metric to compare the performance of the TC, FDE TC, AKF TC, and the proposed methods. The errors of the position, calculated from the candidate algorithms, are shown in Figure 3. The RMSE of the positions for the candidate algorithms are represented in

Table 3. Comparison of algorithm performance between TC, FDE TC, AKF TC, and the proposed algorithm in the different fault scenarios.

Scenarios	TC	FDE TC		AKF TC		Proposed Algorithm
	RMSE (m)	RMSE (m)	Improvement (%)	RMSE (m)	Improvement (%)	RMSE (m)	Improvement (%)
1	9.62	4.92	48.89	6.26	34.97	2.98	69.07
2	13.04	4.92	62.28	6.27	51.89	2.98	77.17
3	17.36	11.28	35	6.78	60.92	2.98	82.85
4	20.73	2.98	85.64	6.82	67.10	2.98	85.64

.

It can be seen, in Figure 3, that TC position error increases rapidly after pseudorange errors are introduced in the four scenarios. This indicates that, without FDE, the IMU/GNSS integrated navigation positioning quality is seriously degraded and results in divergence in the filter estimated results. Therefore, quality control of the GNSS measurement is essential. Meanwhile, by observing the position errors of the FDE TC in different scenarios, it can be seen that, in most cases, when two satellites simultaneously fail, the performance of FDE TC is poor. Only when one satellite has a 30 m step error, and one satellite has a 50 m step error, does FDE TC correctly identify the two faulty satellites in all epochs and eliminate them.

In the other three scenarios, however, the corresponding faulty satellites could not be correctly detected and excluded in all epochs by FDE TC, resulting in a large positioning error. In scenario 3, the maximum positioning error of the FDE TC method even exceeds that of the traditional TC. This is mainly because, in scenario 3, the two satellites add the same step error. As a result, the test statistics of other satellites are strongly correlated with the two faulty satellites, resulting in the maximum test statistics exceeding the traditional w-test threshold. When the satellite with the maximum test statistics exceeding the threshold is eliminated based on a traditional w-test, the redundancy of the observation data is further reduced, so the remaining faulty satellite cannot be detected in the subsequent traditional w-test. The satellite faults still exist in the GNSS measurements, so the positioning performance of the FDE TC is comparable to that of the traditional TC without FDE. It can be seen from Table 3 that the FDE TC, in the above four different scenarios, has similar accuracy to the traditional TC in some cases. However, in scenarios 1 and 2, the FDE TC can still eliminate all faulty satellites in some epochs, but the faulty satellites cannot be correctly eliminated all the time by FDE TC. As a result, the positioning performance of FDE TC is improved by 49% and 62% compared with the traditional TC, respectively. On the other hand, although AKF TC cannot eliminate faults, it reduces the weight of fault observations, thus ensuring the navigation performance to a certain extent. The positioning performance of AKF TC is improved by 35%, 52%, 61%, and 67% compared with the traditional TC, respectively.

However, compared with the above algorithm, the proposed algorithm significantly improves positioning accuracy. This also shows that the proposed algorithm can correctly detect the satellites with the step errors in the above four different cases. The 3D positioning RMSE of the algorithm proposed in this paper, in four different fault scenarios, is 2.98 m. Compared with 9.62 m, 13.04 m, 17.36 m, and 20.73 m of the traditional TC, the accuracy is improved by 69.07%, 77.17%, 82.85%, and 85.64%, respectively. In summary, the above results show that the algorithm proposed in this paper can correctly detect the faulty satellites in the real-data field scenarios with the simulated step errors. Compared with the traditional TC, FDE TC, and AKF TC, it is able to provide a significant improvement in the position solutions.

3.2. Field Test

In order to further validate the performance of the proposed algorithm in an urban environment, a field test was carried out in a deep urban environment in Taipei. The experimental data acquisition equipment contained a low-cost IMU Stim-300 and a GNSS receiver Trimble BD 982, with a sampling rate of 250 Hz and 1 Hz, respectively. The reference trajectory was obtained by an integrated high-grade GNSS receiver and iNAV-RQH IMU with the commercial software NovAtel Inertial Explorer. The experimental test environment is shown in Figure 4, and the reference trajectory is shown in Figure 5. PDOP values during the test are always very high, with the highest value above 16, exhibiting the characteristics of the deep urban environment, as seen in Figure 6. The number of visible satellites is shown in Figure 7.

In order to evaluate the performance of the proposed algorithm, the results of the proposed algorithm are compared with those of the traditional TC, FDE TC, and AKF TC. The errors in position, velocity, and altitude, calculated from the algorithms, are shown in Figure 8, Figure 9 and Figure 10. The accuracies (RMSE) of the position, velocity, and altitude for the algorithms are given in Table 3,

Table 4. The position RMSE of TC, FDE TC, AKF TC, and the proposed algorithm in field test.

Algorithm	RMSE (m)
	North	East	2D	Down
TC	3.31	3.72	4.98	10.79
FDE TC	2.65	3.15	4.11	9.66
Improvement over TC (%)	19.94	15.32	17.47	10.47
AKF TC	2.94	3.27	4.40	8.94
Improvement over TC (%)	11.18	12.10	11.65	17.15
Proposed algorithm	2.55	2.80	3.79	7.51
Improvement over TC (%)	22.96	24.73	23.90	30.40
Improvement over FDE TC (%)	3.77	11.11	7.79	22.26
Improvement over AKF TC (%)	13.27	14.37	13.86	15.88

,

Table 5. The velocity RMSE of TC, FDE TC, AKF TC, and the proposed algorithm in field test.

Algorithm	RMSE (m/s)
	North	East	2D	Down
TC	0.68	0.71	0.98	1.07
FDE TC	0.48	0.55	0.73	0.93
Improvement over TC (%)	29.41	22.54	25.51	13.08
AKF TC	0.63	0.63	0.89	1.21
Improvement over TC (%)	7.35	11.27	9.18	−13.08
Proposed algorithm	0.45	0.38	0.59	0.72
Improvement over TC (%)	33.82	46.48	39.80	32.71
Improvement over FDE TC (%)	6.25	30.91	19.18	22.58
Improvement over AKF TC (%)	28.57	39.68	33.71	40.50

and

Table 6. The altitude RMSE of TC, FDE TC, AKF TC, and the proposed algorithm in field test.

Algorithm	RMSE (°)
	Pitch	Roll	Yaw
TC	2.70	1.39	3.43
FDE TC	2.62	1.38	2.28
Improvement over TC (%)	2.96	0.72	33.53
AKF TC	2.33	1.43	3.08
Improvement over TC (%)	13.70	−2.88	10.20
Proposed algorithm	2.58	1.27	2.25
Improvement over TC (%)	4.44	8.63	34.4
Improvement over FDE TC (%)	1.53	7.97	1.32
Improvement over AKF TC (%)	−10.73	11.19	27.60

.

From Figure 8 and Table 3, the AKF TC position RMSE is 4.40 m in the horizontal direction and 8.94 m in the vertical direction (Down), which is an improvement of 11.65% and 17.15% compared to the 4.98 m and 10.79 m of the TC. The FDE TC vertical position RMSE is 9.66 m, whose performance is not as good as AKF TC, but the performance is better in the horizontal direction. However, neither is as much improved as the algorithm proposed in this paper. The position RMSE of the algorithm proposed is 3.79 m and 7.51 m in the horizontal and vertical directions. The results represent improvements of 23.90% and 30.40% compared to TC without FDE, 7.79% and 22.26% over FDE TC, as well as 13.86% and 15.88% over AKF TC, respectively. As shown in Figure 11, the algorithm proposed in this paper has a better performance in urban environments in the horizontal directions.

It can be seen from Figure 9 and Table 5 that the horizontal and vertical velocity RMSE of the traditional TC scheme without FDE are 0.98 m/s and 1.07 m/s, with the corresponding values, from the proposed algorithm, of 0.59 m/s and 0.72 m/s. These correspond to improvements of 40% and 33%, respectively. While the AKF TC gives an RMSE for horizontal velocity of 0.89 m/s, the performance in the vertical direction deteriorates by 13.08% due to its inability to be accurately adjusted, specifically, for the errors caused by multipath signals and NLOS that are common in urban areas. Compared with the 0.73 m/s and 0.93 m/s of FDE TC, the proposed algorithm in this paper improves by 19% and 23%. This shows that correct fault detection and elimination is effective for quality control.

For the performance of altitude determination in Figure 10 and Table 6, pitch, roll, and yaw RMSE of the traditional TC scheme without FDE are 2.70°, 1.39°, and 3.43°, with the corresponding values from the FDE TC of 2.62°, 1.38°, and 2.28°. These correspond to improvements of 2.96%, 0.72%, and 33.53%, respectively. It is worth noting that the correction of yaw information has always been a difficult problem in the GNSS/IMU integrated navigation algorithm, and the yaw RMSE of FDE TC has dropped by 27.6%. This further illustrates the importance of quality control. While the AKF TC gives an RMSE for pitch angle of 2.33°, the performance in the roll angle deteriorates by 2.88%, and there is less improvement in the yaw angle. The proposed algorithm has improved the estimation results of pitch angle, roll angle, and yaw angle by 2%, 8%, and 1% compared with FDE TC, respectively. Although the performance of the proposed algorithm in this paper is not good in the pitch angle, compared with AKF TC, the overall performance of the proposed algorithm in this paper is better, which improves by 11.19% and 27.60% in roll and yaw angles.

4. Conclusions

This paper has developed a dual w-test-based quality control algorithm for IMU/GNSS integrated navigation in urban areas. Simulation and field test results show that the proposed algorithm is capable of achieving quality control for integrated IMU/GNSS navigation. The experimental results in deep urban environments show that the proposed integration algorithm can improve positioning accuracy compared to the cases without fault exclusion by about 24% and 30%, compared to FDE TC by about 8% and 22%, and compared to AKF TC by about 14% and 16% in the horizontal and vertical directions, respectively. However, the current work does not suit for the case of insufficient visible satellites, as the dual w-test cannot be carried out without enough of a degree of freedom in the statistic SSE. In future work, we will continue to develop more advanced quality control methods, including seeking a better robust algorithm when the number of satellites is insufficient and designing a corresponding failure detection algorithm according to the failure mechanisms of different sensors, such as inertial sensors, vision sensors, and lidar.