*5.3. Case Two: The Wide Floor Hole*

In case two, the GNSS signal was seriously affected by multipath and NLOS signals when the vehicle went through the wide floor hole. The trajectory of the vehicle during data collection is shown in Figure 16a. The vehicle circled twice around the red trajectory in Figure 5b. In our analysis, the positioning error is higher in the period from the 179th to 198th s and from the 377th to 401st s. It happens to be similar according to the onboard RTK data, the solution types of the GNSS positioning results in this period were not "NARROW INT". It is believed that the GNSS was affected by multipath and NLOS signals. The positioning errors induced in the east and north during data collection are

shown in Figure 16b. The maximum errors in the east and north were −4.2601 m and −19.5848 m, respectively.

**Figure 16.** Trajectory and positioning errors in the east and north. (**a**) The trajectory of the vehicle in case 2. (**b**) The positioning errors in the east and north.

Figure 17 shows the top view of the 3D LiDAR global point cloud map built by the LOAM algorithm. Figure 18 shows 26 tree trunks and lampposts marked as detection targets in the 3D LiDAR global point cloud map. They are marked with colored points in Figure 18 and numbered from 0–25.

**Figure 17.** Three-Dimensional LiDAR global point cloud map in case two.

**Figure 18.** Detection targets in the map numbered 0–25, including tree trunks and lampposts.

Figure 19 shows the results of single frame target detection. We took two frames as the vehicle went through the floor hole. Figure 19a shows the 1742nd frame, and a total of nine targets were detected, including targets numbered twenty-three to twenty-four marked in Figure 18. Figure 19b shows the 1856th frame, and a total of twelve targets were detected, including targets numbered twenty-three to twenty-five marked in Figure 18.

(**a**) (**b**)

**Figure 19.** Single frame point cloud target detection results. (**a**) The 1742nd frame and No. 23–24 targets in Figure 18. (**b**) The 1856th frame and No. 23–25 targets in Figure 18.

Figure 20a shows the matched targets between the single frame target detection outputs and the global map-based target detection results. Figure 20b shows the number of detected targets and matched targets in each frame. It can be seen that at least one matched target was observed in any epoch. This outcome meets the time continuity requirement of matched targets and can support subsequent fault detection processes. Table 5 summarizes the number of detected targets and matched targets obtained with single frame target detection.

**Figure 20.** Matched targets between the single frame and map-based target detection results. (**a**) The No. 0–25 of matched targets for target detection in the global map. (**b**) The number of detected and matched targets in each frame.

**Table 5.** The number of detected targets and matched targets.


The position deviations of all matched targets in each frame in the east and north directions are shown in Figure 21a,b, respectively. Figure 22 shows the mean position deviation of the matched targets in each frame, which is used as the test statistic for the proposed fault detection algorithm.

**Figure 21.** The position deviations of the single frame matched targets in the east and north. (**a**) The position error of the matched targets in the east. (**b**) The position error of the matched targets in the north.

**Figure 22.** The mean position deviations of the matched targets in the east and north in case two.

Similar with Figure 13, Figure 23a shows the fault detection results produced by the residual chi-square test. The results of the two fault detection algorithms are locally magnified in Figure 24a,b. Figure 23b is the result of the proposed LiDAR aided real-time fault detection algorithm. The results of the two fault detection approaches are locally magnified in Figure 24c,d. The fault detection performances of the two algorithms are compared in Table 6. The response time of fault disappearance was 8.94 s and 6.62 s for the residual chi-square test, but they were reduced by 5.465 s on average with the proposed algorithm. Therefore, the low-sensitivity problem of the residual chi-square test for the fault disappearance case is effectively ameliorated. Furthermore, the percentages of false alarm and missed detection were 6.26% and 0.69% for the proposed LiDAR aided real-time fault detection algorithm, which are reductions of 76.49% and 79.03% relative to the residual chi-square test results, respectively.

**Figure 23.** Fault detection results of the residual chi-square test and the proposed algorithm. (**a**) The fault detection results of the residual chi-square test. (**b**) The LiDAR aided real-time fault detection results.

**Figure 24.** Local magnifications of Figure 23 for the fault detection algorithm. (**a**) The first fault detected by the residual chi-square test. (**b**) The second fault detected by the residual chi-square test. (**c**) The first fault detected by the LiDAR aided fault detection algorithm. (**d**) The second fault detected by the LiDAR aided fault detection algorithm.



Similar with Figure 14, the positioning errors of the EKF, OFFAF and the proposed algorithm in the east and north for case two are shown in Figure 25. The results are analyzed in Table 7. Compared with the EKF, the mean, maximum and RMSE positioning errors of the proposed algorithm were reduced by 7.4%, 20.49% and 12.81% in the east and 79.12%, 68.22% and 73.31% in the north, respectively, during the fault period. Compared with the OFFAF, the mean, maximum and RMSE positioning errors of the proposed algorithm were reduced by 26.2%, 3.7% and 12.98% in the east and 51.7%, 18.8% and 35.1% in the north, respectively, during the fault period. The proposed algorithm is more effective than the OFFAF.

**Figure 25.** Positioning errors of the proposed algorithm in the east and north in case two.



Similar with Figure 15, the error bounds and positioning errors induced by the EKF and the proposed algorithm in the horizontal direction in case two are shown in Figure 26a,b, respectively. The error bounds of the EKF could not overbound the horizontal errors in some epochs. However, the error bound of the proposed algorithm could overbound the horizontal error in all epochs. From Table 8, the error bounds failed to overbound the horizontal error in 1490 epochs. The mean value and maximum value of the error bounds yielded by the proposed algorithm were reduced by 56.35% and 73.2%, respectively, relative to the EKF during the fault period. Therefore, the integrity of the system could be guaranteed by the proposed algorithm.

**Figure 26.** The error bounds and horizontal error of the EKF and the proposed algorithm in case 2. (**a**) The error bounds and horizontal error of the EKF. (**b**) The error bounds and horizontal error of the proposed algorithm.

**Table 8.** The error bounds of the EKF and the proposed algorithm during the fault period.


#### *5.4. Discussion*

In our research, the LiDAR aided GNSS/INS integration fault detection and localization algorithm is proposed. The proposed fault detection algorithm could effectively improve the sensitivity of residual chi-square test when the fault disappears. The response time for fault disappearance is an important indicator. The localization algorithm could reduce the positioning error compared to EKF. Finally, the integrity of the proposed algorithm is evaluated including false alarm rate, missed detection rate and error bounds. The algorithms proposed in this paper are oriented to the autonomous driving for level four (L4) or level five (L5). However, due to the limitation of experimental conditions, some problems should be considered.

Firstly, in terms of prior point cloud map, the map constructed by LOAM has errors. However, in our experiments, the map is built on a small scene and the error of the map established by LOAM is less than 0.3 m, which is very small compared with the error caused by multipath and NLOS. Therefore, the error of LOAM is acceptable and it has little effect on the performance of the algorithm. The cost of construction of a prior global point cloud map is high only for the proposed algorithm. However, a high-precision point cloud map is an indispensable part of autonomous localization in the existing research. In the future, the HD map can be produced in the industry which is cheaper and more accurate. The proposed algorithm can be applied with the help of HD map and produce more effect. The map is established offline and will not affect the real-time performance of our algorithm. Storing maps takes up a lot of memory, but is not a major direction in our research. We believe this problem of memory will be solved with the autonomous driving implementation.

Secondly, in terms of point cloud processing being involved in target detection, in this paper, the target detection of a single frame point cloud is carried out by clustering. Object detection of single frame is required in the perception module of autonomous driving. Our algorithm can reuse relevant results in the application process. Therefore, we believe that real-time performance could be guaranteed when the autonomous driving is implemented. In the meanwhile, time delay is also our main research direction in the future, and proposed algorithms need more lightweight processing. The research on the time delay model in [46–48] provides us with a good reference.

Thirdly, in terms of target matching, in this paper, target matching is used instead of scan matching. On the one hand, the target is generally marked in the high-precision semantic map of autonomous driving. We hope that the proposed algorithm can make better use of high-precision semantic map information in future autonomous driving applications. On the other hand, the real-time performance of the proposed algorithm is considered. There are many feature points to be matched on scan matching, reducing the efficiency of the algorithm. Therefore, we apply target matching.

Fourthly, in terms of GNSS/INS integration device and ground truth. In our research, we focus on solving the error problem of the low-cost GNSS/INS integration device so that the low-cost device has a better application value in autonomous driving. The NovAtel SPAN-CPT7 is also affected by multipath and NLOS, but it is more expensive and the positioning accuracy is higher compared to the Newton-M2. Therefore, the SPAN CPT7 is used for providing the ground truth. We are also looking forward to better ground truth solutions in dynamic scenarios for urban canyons positioning.

Finally, in terms of data collection. Due to the limitation of experimental conditions, we conducted tests in two scenarios in this paper with small data samples. In the future, we will collect a large amount of data in urban canyons to verify the performance of our algorithm and make further improvements.
