**1. Introduction**

A reliable positioning system is the basis of autonomous driving [1]. Integrity is an important indicator for ensuring the driving safety of vehicles. The integration of the Global Satellite Navigation System (GNSS) and the Inertial Navigation System (INS) could provide real-time and high-precision positioning and this approach is widely used in military and civil fields [2,3]. However, positioning and navigation in urban canyons is still a challenge [4] because multipath GNSS signals are received due to reflection or non-line-ofsight (NLOS) signals are received due to diffraction; this eventually leads to unacceptable GNSS measurement errors [5–7]. The comprehensive positioning performance of the GNSS/INS integrated navigation system is seriously degraded. Therefore, to meet the positioning performance requirements in urban canyons, such as viaducts, floor holes and

**Citation:** Wang, Z.; Li, B.; Dan, Z.; Wang, H.; Fang, K. 3D LiDAR Aided GNSS/INS Integration Fault Detection, Localization and Integrity Assessment in Urban Canyons. *Remote Sens.* **2022**, *14*, 4641. https:// doi.org/10.3390/rs14184641

Academic Editors: Yuwei Chen, Changhui Jiang, Qian Meng, Bing Xu, Wang Gao, Panlong Wu, Lianwu Guan and Zeyu Li

Received: 28 July 2022 Accepted: 14 September 2022 Published: 16 September 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

tunnels, the real-time detection of GNSS/INS integrated system positioning performance is essential. However, in urban canyons, there are a lot of satellites affected by NLOS and multipath. With all four core GNSS constellations of the world in operation, the number of affected satellites is ten or more. It is difficult to detect all the affected satellites and exclude them from the localization solution by traditional algorithm such as Multiple Solution Separation (MSS). Therefore, we detect faults in the positioning domain. After faults are detected, a GNSS/INS integration algorithm can be designed based on the detection results to improve the positioning accuracy and ensure the integrity of the system.

GNSS fault detection methods are mainly divided based on whether to detect specific faulty satellites [8]. Blanch et al. only used GNSS measurement information to propose a method combining a greedy search and L1 norm minimization to detect satellites with pseudo range errors greater than 20 m that were affected by multipath and NLOS signals [9]. However, in urban canyons, rapid changes in observation satellites reduced the performance of the method. Sun et al. proposed a dynamic detection and multiple fault elimination algorithm based on pseudo range comparison [10]. Using inertial measurement unit (IMU) and GNSS pseudo range data, a parallel fault detection and exclusion scheme consisting of a sliding window and a detector was designed, which can detect multiple faulty satellites in real-time. This scheme is suitable for the tightly and loosely coupled architecture. Groves et al. proposed a likelihood-based 3D-mapping-aided (3DMA) GNSS ranging algorithm and signals, which are predicted to be non-line-of-sight (NLOS) to contribute to the position solution without explicitly computing the additional path delay due to NLOS reception, which is computationally expensive [11]. Sun et al. proposed a new measurement noise covariance update scheme, with the adaptive indicator generated from pseudo range error prediction results, for a tightly coupled GNSS/IMU navigation system in urban areas [12]. Shytermeja and Attia et al. proposed the method of using a fisheye camera to suppress NLOS signals and eliminate influenced satellites [13,14]. Fisheye images are divided into sky and non-sky regions. The GNSS fault detection can be realized by identifying GNSS satellites in the non-sky region received by the receiver. However, satellites affected by multipath signals cannot be excluded by this method. Moreover, the processing technique used for camera data is complex. Wen et al. proposed a similar approach using a real-time 3D point cloud or a sliding window map for identifying edges on the top of the given building [15,16]. Based on the relationship between the edges on the top of the building or double-decker bus and all observed satellites, the satellites influenced by the NLOS signals are detected and pseudo ranges are corrected. However, only the satellites affected by NLOS signals are detected, and the environmental conditions are highly demanding. All four of the above methods need to detect specific affected satellites and exclude them from the positioning solution. However, in urban canyons, lots of satellites are affected by NLOS and multipath signals, so it is difficult to exclude them individually. The residual chi-square test is widely used for fault detection; it constructs a fault detector via the innovation of the Kalman filter (KF) [17]. The test is performed at the positioning domain to detect whether a positioning fault is present. The algorithm is efficient and works in real-time, but the sensitivity of the residual chi-square test is poor when faults disappear. As mentioned above, although the effectiveness of the existing fault detection algorithms for detecting satellites affected by multipath and NLOS signals in complex environments has been proven, challenges remain concerning the real-time performance of dynamic applications [18,19]. Therefore, it is necessary to propose a new real-time fault detection algorithm to improve the sensitivity of the residual chi-square test to fault disappearances, and the integrity of this approach should be assessed in terms of its false alarm rate and missed detection rate. LiDAR is an important sensor in autonomous driving localization, which has good performance in ranging and perception [20]. As the cost of solid-state LiDAR decreases, LiDAR has a wider range of applications [21]. LiDAR has robust performance in building high-precision map and object detection for autonomous driving [22,23]. In this paper, we carry out fault detection algorithm aided by LiDAR.

The KF is a famous recursive algorithm for discrete linear systems and has been widely used in many fields [24]. When the given system is linear and the observed noise follows a Gaussian distribution, the KF can be proven to be optimal. However, Chang et al. pointed out that the noise probability does not obey a Gaussian distribution in practice, and it is difficult to determine the dynamic model and statistical information of the noise distribution [25]. Many scholars have studied a series of filters for non-Gaussian-distributed noise. Gordon et al. developed a particle filter (PF) to address the problem of non-Gaussian Bayesian estimation [26]. However, high-dimensional state estimation may result in a heavy computational burden. The H∞ filter was proposed for the uncertainty of measurement noise, but Rigatos pointed out that it could not solve the problem that GNSS-measured values were outliers [27]. The fading filter and adaptive filter are two widely used filters that have been proposed to solve the uncertainty of the noise distribution characteristics of dynamic systems. Chen et al. analyzed the difference between the adaptive filter and fading filter in detail. The fading filter mainly sets the weight of the state covariance matrix, while the adaptive filter mainly sets the weight of the measured noise [28]. Fagin and Sorenson proposed the fading filter algorithm in the 1960s [29]. Lee and Sun et al. proposed utilizing the fading filter method in the field of integrated navigation [30,31]. Although these methods improve the performance of the KF and the resultant positioning accuracy, many matrix multiplication and inversion operations are involved, resulting in poor practicability. Li et al. proposed a dynamic fading filter algorithm to solve problems with difficult matrix multiplication operations, greatly improving the performance of the algorithm [2]. Li et al. proposed a fading filter to defend against outliers. When a GNSS fault occurred, the innovation of the last n epochs was used to estimate the innovation of the current epoch, but the weights of the last n epochs were not considered [32]. Wang et al. developed the Sage–Husa adaptive filter, designed a sliding window and determined the covariance matrix of the current epoch based on an iterative method, but it was quite difficult to choose the window length [33]. Zhou et al. proposed a new adaptive unscented KF (N-AUKF) algorithm [34]. A covariance matching criterion was designed to judge the filtering divergence, and an adaptive weighted coefficient was applied to restrain the divergence. However, this approach requires adequate measurements. Li et al. extended this definition to Kalman filtering to detect gross errors, explained its nature and its relation with the currently adopted Chi-square variables of Kalman filtering in model and data spaces and compared them with the predictive residual statistics, starting by defining an incremental chi-square method of recursive least squares [35]. All of the above algorithms are effective in complex environments. However, the measurement noise is estimated by prior modeling with a certain error. Therefore, it is necessary to estimate the measurement noise with multi-sensor redundant measurement information.

Integrity is an important indicator of GNSS positioning in the aviation field [36]. Integrity evaluation from the GNSS to the GNSS/INS integrated positioning. Protection level is an important index of integrity monitoring, which represents the upper limit of positioning error [37]. However, in the existing research, the protection level in GNSS/INS integrated positioning is mainly based on simulation experiments [38], so it is necessary to carry out the verification protection level using the real test data. The protection level can meet the application requirement of autonomous driving.

The main contributions of this paper are as follows. (1) Regarding the problem that the residual chi-square test has low sensitivity when faults disappear, a LiDAR aided real-time GNSS/INS integrated fault detection algorithm based on the characteristics of high-precision 3D LiDAR is proposed. The test statistic is constructed based on the mean position deviation of the matched targets, and the dynamic threshold is constructed by a sliding window based on the power series. (2) To solve the problem that the measurement noise is estimated by prior modeling with a certain error, a LiDAR aided measurement noise estimation and filtering algorithm is proposed. The mean position deviations of the matched targets for the last n epochs are normalized, and an adaptive weight sequence is built. The adaptive measurement noise factor is joined with the Extended Kalman Filter (EKF) innovation covariance. (3) For the problem that the integrated navigation error bounds are calculated by simulations in the existing research, we innovatively verify the error bounds of the integrated navigation system on real test data and optimize the algorithm of error bounds by using the filter in (2).

The subsequent sections of this paper are organized as follows. An overview of the proposed algorithm is given in Section 2. The proposed LiDAR aided real-time fault detection algorithm is presented in Section 3. In Section 4, the LiDAR aided measurement noise estimation adaptive filter algorithm is provided. Two experiments are performed to verify the effectiveness of the proposed algorithm, and its integrity is assessed in Section 5. Finally, Section 6 presents the conclusions and directions for future studies.
