# GNSS and LiDAR Integrated Navigation Method in Orchards with Intermitt GNSS Dropout
## 2 Materials and Methods
### GNSS-LiDAR Fusion Positioning Principle
#### 2.2.2 GNSS-Corrected Laser Odometer  
The GICP algorithm improves the accuracy and robustness of the registration between point cloud data by considering the position uncertainty of each point. Compared with the traditional ICP algorithm, GICP not only incorporates the Gaussian distribution information of points in the solution process but also uses KD-tree to efficiently search for the nearest point, which greatly accelerates the registration speed. Suppose the source point cloud \(P^{s}=\left\{P_{1}^{s},P_{2}^{s},\ldots,P_{n}^{s}\right\}\) and the target point cloud \(P^{t}=\left\{P_{1}^{t},P_{2}^{t},\ldots,P_{n}^{t}\right\}\), which are registered by the estimated transformation matrix \(\hat{X}\). \(\hat{X}\) belongs to the special Euclidean group \(SE(3)\), consists of a rotation matrix and translation vectors, and can be expressed as \(\hat{X}\in SE(3)\).  
\[p_{i}^{t}=\hat{X}\times p_{i}^{s} \tag{13}\]  
Figure 5: Planar coordinate system diagram.  
Since real-world measurements include noise, the location of each point cloud is assumed to be the mean of the estimated position that obeys the point. The covariance matrix of the point is the Gaussian distribution of the covariance. \(\hat{p}_{i}^{s}\) and \(\hat{p}_{i}^{t}\) are the mean values of the source point cloud \(P^{s}\) and the target point cloud \(P^{t}\), respectively. \(C_{i}^{p^{s}}\) and \(C_{i}^{p^{t}}\) are the covariance matrices of the source point cloud \(P^{s}\) and the target point cloud \(P^{t}\), respectively. The source point cloud \(P^{s}\) and the target point cloud \(P^{t}\) are the Gaussian distribution:  
\[p_{i}^{s} \sim N\left(\hat{p}_{i}^{s},C_{i}^{p^{t}}\right) \tag{14}\] \[p_{i}^{t} \sim N\left(\hat{p}_{i}^{t},C_{i}^{p^{t}}\right) \tag{15}\]  
The alignment error \(d_{i}\) for each pair of corresponding points can be obtained as  
\[d_{i}=p_{i}^{t}-\hat{X}p_{i}^{s} \tag{16}\]  
Since \(p_{i}^{s},\ p_{i}^{t}\) are Gaussian distributions independent of each other, then \(d_{i}\) also belongs to a Gaussian distribution:  
\[d_{i}\sim N\left(0,C_{i}^{p^{t}}+\left(\hat{X}\right)C_{i}^{p^{s}}\left(\hat{X }\right)^{T}\right) \tag{17}\]  
The iterative calculation of \(\hat{X}\) is obtained by the maximum likelihood estimation (MLE) method:  
\[\hat{X}=arg\underset{\hat{X}}{max}\left(\hat{X}P^{s},P^{t}\right) \tag{18}\]  
where the residual error \(\varepsilon\) is defined as  
\[\varepsilon\left(\hat{X}P^{s},P^{t}\right)=\sum_{i}^{n}d_{i}^{T}\left(C_{i}^{ p^{s}}+\hat{X}C_{i}^{p^{t}}\hat{X}^{T}\right)^{-1}d_{i} \tag{19}\]  
The GICP algorithm has a wider range of applications than the classical ICP algorithm, and the GICP alignment is better than the ICP in terms of matching speed and accuracy [28]. Therefore, this paper chooses the algorithm based on GICP matching for point cloud alignment. The structure of the laser odometer workflow is shown in Figure 6.  
Figure 6: Laser odometer structure.  
After the work described above, we obtained descriptions of GNSS and LiDAR positioning information under a unified coordinate system. In actual use of the LiDAR odometry based on generalized iterative closest point (GICP), we discovered that although its cumulative error is smaller compared to INS, it can still lead to shifts in positioning information over long periods of use. This shift phenomenon is especially noticeable when changing rows, as illustrated in Figure 7a. To enhance the navigation accuracy of the LiDAR odometer in the orchard, we integrate the GNSS positioning information after coordinate alignment to correct and initialize the LiDAR odometer at the end of each row. After reaching the next transverse end of the field, initialization is stopped, and the current frame is used as the initial frame, taking the GNSS output at this moment as the pose information for the initial frame. Then, the keyframes in the stored keyframe library are cleared to eliminate accumulated errors. The effect of the GNSS-corrected LiDAR odometer is shown in Figure 7b.