*4.2. LiDAR Aided Real-Time Measurement Noise Estimation of Adaptive Filter*

Single fading factor adaptive filter and optimal fading factor adaptive filter algorithms are used to solve the uncertainty of measurement noise when GNSS is affected by multipath and NLOS. The above algorithms can realize the optimal estimate in theory, but are based on prior modeling. The real measurement noise is not considered.

Taking advantage of the above two algorithms and considering the specific problem, we propose a LiDAR aided real-time measurement noise estimation adaptive filter algorithm. In the proposed algorithm, a sliding window with a length of *n* is constructed. The data in the sliding window are determined by the normalization of the mean position deviation of the matched targets. The innovation of the filter at the fault epoch is estimated in real-time. The value of the adaptive measurement noise factor is determined based on

the strict theoretical derivation of the filter with the optimal fading factor. Real-time measurement noise estimation is realized. Finally, the accuracy and robustness of GNSS/INS integrated positioning are improved.

When faults are detected by the LiDAR aided real-time fault detection algorithm, the LiDAR aided real-time measurement noise estimation adaptive filter is applied for GNSS/INS integration. When faults are not detected, the EKF is applied. The algorithm flow chart is shown in Figure 3.

**Figure 3.** Schematic diagram of the LiDAR aided measurement noise estimation adaptive filter.

The specific implementation of the LiDAR aided real-time measurement noise estimation adaptive filter algorithm is shown in the following steps.

(1) An adaptive measurement noise factor *λ<sup>k</sup>* is added to the innovation covariance **A***<sup>k</sup>* to produce Equation (27):

$$\mathbf{A}\_{k} = \mathbb{E}\left(\mathbf{y}\_{k}\mathbf{y}\_{k}^{T}\right) = \mathbf{H}\_{k}\mathbf{P}\_{k/k-1}\mathbf{H}\_{k}^{T} + \lambda\_{k}\mathbf{R}\_{k} \tag{27}$$

(2) According to the filter convergence conditions for Equations (23) and (24), the adaptive measurement noise factor *λ<sup>k</sup>* can be calculated [43].

$$\lambda\_k = \frac{\mathbf{y}\_k^T \mathbf{y}\_k - \text{tr}\left(\mathbf{H}\_k \mathbf{P}\_{k/k-1} \mathbf{H}\_k^T\right)}{\text{tr}(\mathbf{R}\_k)}\tag{28}$$

(3) The filter innovation in a fault epoch is calculated based on the sliding window. *λ<sup>k</sup>* is only related to γ*<sup>k</sup>* from Equation (28), and γ*<sup>k</sup>* in the *k*th epoch is constructed by selecting the γ obtained with the previous n epochs to construct a sliding window, which is defined as {γ*k*−1, γ*k*−<sup>2</sup> ··· , γ*k*−*n*}. The adaptive weight sequence is ¯

selected as {*βi*} = {*β*1, *β*2, ··· *βn*}. The mean value Δ **X** of the corresponding epochs ) <sup>Δ</sup>*Xk*−<sup>1</sup> *target*, <sup>Δ</sup>*Xk*−<sup>2</sup> *target* ··· , <sup>Δ</sup>*Xk*−*<sup>n</sup> target*\* is normalized, and the adaptive weight sequence is determined.

$$\beta\_i = \frac{\Delta X\_{\text{target}}^i - \Delta \overline{X}}{\max \left\{ \Delta X\_{\text{target}}^i \right\} - \Delta \overline{X}} \tag{29}$$

$$\mathbf{y}\_{k} = \beta\_{1}\mathbf{y}\_{k-1} + \beta\_{2}\mathbf{y}\_{k-2}\cdots + \beta\_{n}\mathbf{y}\_{k-n} \tag{30}$$


#### **5. Experimental Results and Discussion**

*5.1. Introduction to the Experiment*

5.1.1. Sensor Setups

In both experiments, Newton-M2, a low-accuracy GNSS/INS integrated navigation system, was used to collect real-time kinematic (RTK) GNSS data at a frequency of 1 Hz and INS data at a frequency of 100 Hz. A 3D LiDAR sensor (Velodyne 16) was to collect raw 3D point clouds at a frequency of 10 Hz. In addition, the NovAtel SPAN-CPT7, a high-accuracy GNSS (GPS, Global Navigation Satellite System (GLONASS), and Beidou) RTK/INS integrated navigation system, was used to provide the ground truth positioning data. The RTK is onboard and runs in real-time. The NovAtel SPAN-CPT7 is also affected by multipath and NLOS, but the accuracy is higher and our research focuses on lowprecision and cheap integrated navigation devices. Therefore, the SPAN-CPT7 was used for providing the ground truth. The coordinate systems between all the sensors were calibrated before conducting the experiments. The experimental equipment is shown in Figure 4.

**Figure 4.** The experimental equipment for data collection. (**a**) The vehicle used for data collection and the installation positions of the sensors. (**b**) Local magnification of the Newton-M2 and NovAtel Span CPT7.

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

The Apollo 5.5 was used to collect GNSS and INS data from Newton-M2 and LiDAR data from Velodyne 16. The data format is Protobuf. The Novatel SPAN-CPT7 saves the data through the serial port. The data format is OEM7. The Newton-M2 and the Novatel SPAN-CPT7 are synchronized to the GNSS and INS through GPS timing, and the LiDAR is synchronized by the pulse per second (PPS) of the GPS timing.
