## 2 Research of SLAM Algorithm
### Local Slam  
The front-end stage starts with data extraction, in which two methods, voxel filtering and adaptive voxel filtering, are used to compress the LiDAR data to reduce resource consumption and facilitate subsequent data processing. Due to the large scale of each frame of point-cloud data acquired by LiDAR and influenced by several factors, it can impact the point-cloud processing results. To estimate the trajectory and the bit position of the robot [31], an IMU and an odometer are used for extrapolation, and the estimated value of the bit position is used as the initial value. The LiDAR data are fused with the obtained positional estimates by a traceless Kalman filter [32] to update the positional estimates and to correct the LiDAR data. Each LiDAR scan forms a submap, and each frame of the point cloud is compared with the previous submap, and the pixel probability values in the submap are updated [33]. A motion filter is used to reject sensor frames of data at rest and continuously update the subgraph until a fully optimized subgraph is obtained when no new point clouds are inserted. The pseudo-code of the motion filter is as follows (Algorithm 1). ```
1:Input: Odometry, IMU, LaserScan
2:Output: Filtered motion estimation
3:Initialization:
4: Initialize motion filter parameters
5: Initialize state variables: pose, velocity, covariance matrix
6:
7:Procedure MotionFilter:
8:loop
9: Receive Odometry, IMU, and LaserScan data
10: Predict motion using odometry information
11: Update state variables: pose, velocity
12: Update covariance matrix based on the motion model
13: Correct motion using IMU data
14: Calculate IMU-based motion estimate
15: Compare the estimated motion with the predicted motion
16: Update state variables and covariance matrix based on the correction
17: Fuse LaserScan data with motion estimate
18: Perform scan-matching to align the laser scan with the estimated map
19: Update state variables and covariance matrix based on the scan-matching result
20: Output the filtered motion estimation
21:endloop
22:
23:End Procedure
```  
**Algorithm 1**Motion Filter. The process uses a scan matcher based on the Ceres library to compute the poses of the point cloud, transforming the pose-solving problem into a nonlinear least-squares problem [34]:  
\[\underset{\Xi^{m},\Xi^{s}}{\arg\min}\frac{1}{2}\sum_{ij}\rho\left(E^{2}\left( \xi_{i}^{m},\xi_{j}^{s};\Sigma_{ij},\xi_{ij}\right)\right) \tag{3}\]where \(\Xi^{m}=\left\{\xi_{i}^{m}\right\}\) are the coordinates of the submap in world coordinates; \(\Xi^{s}=\left\{\xi_{j}^{s}\right\}\) is the locus of scan in world coordinates; \(\xi_{ij}\) denotes the poses of the scan under the submap; \(\Sigma_{ij}\) is the corresponding covariance matrix [35]. Residuals \(E\) and the error \(e\):  
\[E^{2}\left(\xi_{i}^{m},\xi_{j}^{s};\Sigma_{ij},\xi_{ij}\right)=e\left(\xi_{i}^ {m},\xi_{j}^{s};\xi_{ij}\right)^{T}\Sigma_{ij}^{-1}e\left(\xi_{i}^{m},\xi_{j}^ {s};\xi_{ij}\right) \tag{4}\]  
\[e\left(\xi_{i}^{m},\xi_{j}^{s};\xi_{ij}\right)=\xi_{ij}-\left( \begin{array}{ c}R_{i\mu}^{-1}\left(t_{\xi_{i}^{m}}-\xi_{j}^{s}\right)\\ \xi_{i\mu\theta}^{m}-\xi_{j\mu}^{s}\end{array} \right) \tag{5}\]  
First, during the loopback detection process, the system performs loopback optimization, i.e., it detects whether there is a loopback or not and then optimizes accordingly. When the current scan data are close enough in the distance to a certain laserscan position in the already created submap, the loopback can be found through a scan-matching strategy. To reduce the computation and improve the efficiency of real-time loop detection, Cartographer applies the branch-and-bound optimization method to optimize the search [36]. If a good-enough match is obtained, the loop-detection part is over by this point. The next step is to optimize the poses in all the submaps based on the current scan's pose and one of the closest matches in the submap, even if the residual \(E\) is minimized.