**2. Overview of the Proposed Algorithm**

An overview of the algorithm proposed in this paper is shown in Figure 1. First, based on LiDAR's high-precision ranging and its perception of the surrounding environment, a LiDAR aided real-time GNSS/INS integrated positioning fault detection algorithm is proposed. A 3D LiDAR global point cloud map based on the LiDAR odometry and mapping (LOAM) [39] algorithm is established in this paper, and the map is processed in Cloud Compare software to mark the detected targets and achieve target detection. The targets in this study are mainly rods, including tree trunks and lampposts. Onboard the LiDAR, real-time scanning of the surrounding environment is carried out to detect the targets of interest based on the single frame point cloud. The targets detected based on this single frame point cloud are matched with the targets marked on the global point cloud map. A test statistic is constructed based on the mean position deviation of the matched targets, and a dynamic threshold is constructed by a sliding window based on the power series. A detector is used for the detection of GNSS/INS integrated positioning faults affected by NLOS and multipath signals. The specific implementation and detailed description of the fault detection algorithm is in Section 3.

**Figure 1.** System framework of the proposed algorithm.

Second, a LiDAR aided real-time measurement noise estimation adaptive filter positioning algorithm is proposed. Based on the proposed fault detection algorithm, the proposed positioning algorithm is used to improve the performance of GNSS/INS integrated positioning. The adaptive measurement noise factor is joined with the EKF innovation covariance. The mean position deviations of the matched targets for the last *n* epochs are normalized, and an adaptive weight sequence is built. Then, the innovation of the current epoch is calculated. Thus, the adaptive measurement noise factor for the current epoch is determined to achieve the proposed filtering and localization algorithm. The specific implementation and detailed description of the localization algorithm are in Section 4.

Finally, the integrity of the proposed algorithm is assessed based on real test data; the evaluation metrics include the missed detection and false alarm rates for fault detection and the error bounds of the localization algorithm. The integrity assessment is realized for the proposed LiDAR aided fault detection and localization algorithm under the influence of NLOS and multipath signals.

#### **3. LiDAR Aided Real-Time Fault Detection Algorithm**

#### *3.1. KF Architecture and the Residual Chi-Square Test*

#### 3.1.1. KF Architecture

The KF is composed of state prediction and measurement update equations, as shown in Equations (1) and (2), respectively [35]. In these equations, **X***<sup>k</sup>* represents the state vector of the system at the *k*th epoch, **Φ***k*/*k*−<sup>1</sup> denotes the state transition matrix of the system from the (*k* − 1)th epoch to the *k*th epoch, **W***<sup>k</sup>* represents the system noise, **Z***<sup>k</sup>* is the measurement vector of the system at the *k*th epoch, **H***<sup>k</sup>* represents the system measurement matrix at the *k*th epoch, **V***<sup>k</sup>* is the measurement noise vector and **Γ***<sup>k</sup>* is the system noise distribution matrix.

$$\mathbf{X}\_{k} = \Phi\_{k/k-1} \mathbf{X}\_{k-1} + \Gamma\_{k} \mathbf{W}\_{k} \tag{1}$$

$$\mathbf{Z}\_k = \mathbf{H}\_k \mathbf{X}\_k + \mathbf{V}\_k \tag{2}$$

In our research, the KF is used for integrating the GNSS and the INS and the loosely coupled architecture is used. According to [40], we estimate a 15-dimensional state vector **X**, which includes the position error *δp*, velocity error *δvn*, attitude error *φn*, static accelerometer bias *δba* and static gyroscope bias *δbg*, each of which possesses a 3D vector. The state vector **X** is given by

$$\mathbf{X} = \begin{bmatrix} \delta p \ \delta v^n \phi^n \delta b \mu \delta b\_{\mathcal{S}} \end{bmatrix}^T \tag{3}$$

The measurement vector **Z** is 3-dimensional, which includes the deviation between the position of GNSS and INS. *pGNSS* and *pINS* are the GNSS position and INS position, respectively. The *pGNSS* is the GNSS position of the output of Newton-M2 receiver with onboard RTK.

$$\mathbf{Z} = [p\_{\rm GNNS} - p\_{\rm INS}] = \begin{bmatrix} p\_{\rm GNNS}^X - p\_{\rm UN}^X \\ p\_{\rm GNNS}^Y - p\_{\rm INS}^Y \\ p\_{\rm GNNS}^Z - p\_{\rm INS}^Z \end{bmatrix} \tag{4}$$

The system noise vector **W** is 6-dimensional, which includes the gyroscope and accelerometer noise. The matrix **Q** is the covariance of **W**.

$$\mathbf{Q} = \operatorname{diag} \begin{bmatrix} \sigma\_{\mathcal{g}R}^2 & \sigma\_{\mathcal{g}F}^2 & \sigma\_{\mathcal{g}II}^2 & \sigma\_{aR}^2 & \sigma\_{aF}^2 & \sigma\_{aII}^2 \end{bmatrix} \tag{5}$$

The measurement noise vector **V** is 3-dimensional, which includes the GNSS uncertainty. The matrix **R** is the covariance of **V**.

$$\mathbf{R} = \operatorname{diag} \begin{bmatrix} \sigma\_X^2 & \sigma\_Y^2 & \sigma\_Z^2 \end{bmatrix} \tag{6}$$

In the GNSS/INS integrated positioning, the **Q** is set by IMU sensor information and the **R** is set by the GNSS uncertainty of the output of Newton-M2 receiver, which changes with time. When the GNSS positioning quality is degraded, the uncertainty becomes significantly larger. The uncertainty of the output is in the Earth-North-Up (ENU) frame and it is transformed to Earth-Centered and Earth-Fixed (ECEF) frame.

When the initial state information, the state vector **X**<sup>0</sup> and the initial value of the onestep prediction covariance matrix **P**<sup>0</sup> is given, the optimal estimation of the state parameters can be achieved according to the time prediction and measurement update processes by using the state prediction and measurement update equations of the system in [31].

### 3.1.2. The Residual Chi-Square Test

According to [17], the test statistic of the residual chi-square test *qk* at the *k*th epoch is constructed as

$$q\_k = \sum\_{i=1}^k \mathbf{y}\_i^T \mathbf{S}\_i^{-1} \mathbf{y}\_i \tag{7}$$

γ*<sup>k</sup>* is the innovation of KF and **S***<sup>i</sup>* is the covariance matrix of γ*<sup>k</sup>* at the *i*th epoch.

In the residual chi-square test, the mean value of innovation is 0 under the no-fault hypothesis H0. The test statistic follows the central chi-square distribution. In contrast, under the fault hypothesis H1, the mean KF innovation value is not 0 and the test statistic obeys the non-central chi-square distribution.

$$\begin{cases} \text{H}\_{0} \colon \text{E}(\mathbf{y}\_{k}) = 0, q\_{k} \sim \chi^{2}(n) \\ \text{H}\_{1} \colon \text{E}(\mathbf{y}\_{k}) \neq 0, q\_{k} \sim \chi^{2}(n, \lambda) \end{cases} \tag{8}$$

For a determined false alarm probability *PFA*, the residual chi-square test threshold TD is determined by the inverse of the chi-square cumulative distribution function (CDF) [3], which is given by

$$P(q\_k \le \mathcal{T}\_\mathcal{D}/\mathcal{H}\_0) = \int\_0^T f\_{\chi^2(n+1)}(\mathbf{x})d\mathbf{x} = 1 - P\_{\mathcal{F}A} \tag{9}$$

The residual chi-square test algorithm is used to detect faults in this study. The performance of this algorithm is compared with that of the proposed algorithm.

#### *3.2. LiDAR Aided Real-Time Fault Detection*

#### 3.2.1. The Theory of the Fault Detection Algorithm

The fault detection algorithm is realized by calculating the mean position deviation of all matched targets. The positions of the detected targets can be determined by establishing a global point cloud map. When no fault occurs, the vehicle can be located by GNSS/INS integrated positioning. The observed position of each target can be obtained by jointly exploiting the single frame point cloud target detection process of LiDAR. The position deviation of the matched targets between the real-time observed positions and the detected targets based on the 3D LiDAR map is very small. When the GNSS signals are affected by NLOS and multipath signals, the GNSS/INS integrated positioning results are biased; thus, large position deviations occur among the matched targets. A schematic diagram of the fault detection procedure aided by LiDAR is shown in Figure 2.

#### 3.2.2. Three-Dimensional LiDAR Mapping and Target Detection Based on a Map

An a priori 3D LiDAR global point cloud map is established based on IMU and LiDAR data produced by LOAM [39]. First, the feature points are extracted, including the corner points and flat points. Thus, the odometry is obtained by interframe matching. Then, the map is obtained by matching the current frame with the built 10 Hz odometry to obtain more accurate mileage information and update the 3D LiDAR map. Rods, including lampposts and tree trunks, are extracted from the 3D LiDAR map using Cloud Compare software [41]. They are used as the targets to be detected based on the 3D LiDAR map.

**Figure 2.** Schematic diagram of the fault detection process aided by LiDAR.

#### 3.2.3. Single Frame Point Cloud Target Detection

Real-time single frame point cloud target detection is mainly achieved based on a point cloud library (PCL) [42]. First, the detection area of the single frame point cloud is set, and then, a voxel grid filter is used to filter the single frame point cloud. Second, the point cloud is segmented based on the road plane and obstacles by random sample consensus (RANSAC). Third, the obstacles on the road are clustered by Euclidean clustering, and a K-dimensional (KD) tree is used to conduct a nearest-neighbor search. Finally, the bounding boxes are associated with the detected targets based on the single frame point cloud [43].
