# A Lidar-Inertial Navigation System for UAVs in GNSS-Denied Environment with Spatial Grid Structures
## 2 Lidar-Inertial Navigation System for UAVs in GNSS-Denied Environment with Spatial Grid Structures
### Implementation of the Lidar Navigation System  
The proposed lidar-inertial navigation system uses the same fusion framework as LIO-SAM in the front end. The main difference is the design of the lidar odometer part of the navigation system.  
To solve the motion of the lidar in a GNSS-denied environment with spatial grid structures, we make certain assumptions about the relationship of the extracted features. The shell feature is assumed to be locally flat. The grid features are assumed to be locally collinear. Then the cost functions of point-to-line and point-to-plane distances are constructed. Compared to the cost function expressions used in LIO-SAM, the proposed methods are compact and easy to construct to solve on SE(3) manifolds.  
For the grid feature \(P_{e}^{L}\), find the five nearest neighbor points corresponding to the feature in the local map \(M_{e,local}^{N}\) composed of grid features. Assume that the five points are collinear. Compute their geometric centers \(\hat{P}_{e}^{N}\) and the direction vector \(\hat{n}_{e}^{N}\) of the target line composed of them. The distance \(d_{e}\big{(}P_{e}^{L}\big{)}\) from the feature point \(P_{e}^{L}\) to the target line is shown in Equation (5).  
\[d_{e}\Big{(}P_{e}^{L}\Big{)}=\alpha\cdot\Big{[}\Big{(}T_{L}^{N}P_{e}^{L}-\hat{ P}_{e}^{N}\Big{)}\times\hat{n}_{e}^{N}\Big{]} \tag{5}\]  
where \(T_{L}^{N}\) is the pose transformation matrix representing the lidar in the navigation frame. \(\alpha\) is the unit vector of the direction vector from the feature point to the target line. The expression of \(\alpha\) is shown in Equation (6).  
\[\alpha=\frac{\big{(}T_{L}^{N}P_{e}^{L}-\hat{P}_{e}^{N}\big{)}\times\hat{n}_{e }^{N}}{\big{\|}\big{(}T_{L}^{N}P_{e}^{L}-\hat{P}_{e}^{N}\big{)}\times\hat{n}_{ e}^{N}\big{\|}} \tag{6}\]  
Similarly, the distance \(d_{s}\big{(}P_{s}^{L}\big{)}\) from the shell feature \(P_{s}^{L}\) to the target plane is shown in Equation (7).  
\[d_{s}\Big{(}P_{s}^{L}\Big{)}=\Big{(}T_{L}^{N}P_{s}^{L}-\hat{P}_{s}^{N}\Big{)} \times\hat{n}_{s}^{N} \tag{7}\]  
where \(\hat{P}_{s}^{N}\) is the geometric center of the nearest-neighbor points, which are assumed to be coplanar, corresponding to the shell feature \(P_{s}^{L}\) in the local map \(M_{s,local}^{N}\) composed of shell features, and \(\hat{n}_{s}^{N}\) is the normal vector of the target plane.  
Figure 5: Example of extracting grid features and shell features. **(a)** shows a GNSS-denied environment with space grid structures, where we operated a UAV with a lidar navigation system flown in. The photo was taken facing north. **(b)** shows the real-time point cloud collected by the lidar, where the color of the point represents the reflection intensity. **(c)** shows the grid features and shell features extracted from the point cloud; the grid features are painted in green and the shell features are painted in purple.  
For all the features, the particular UAV pose at the current moment can be obtained by minimizing the sum of the above cost functions. The output frequency of this pose result is 10 Hz. It is then fused with the IMU using factor graph optimization [24], which is solved by iSAM2 [25].