## 2 System Structures
### Other Factors Modeling  
It is known from Figure 2 that measurements of GNSS and LiDAR are employed to assist IMU positioning in the factor graph integrated navigation system. The GNSS positioning is mainly calculated using the pseudo-range difference method to obtain final information for factor node construction. Assuming that GNSS nodes follow the Gaussian noise distribution, the measurement model of the GNSS can be written as follows:  
\[\mathbf{Z}_{k}^{gnss}=h_{k}^{gnss}(\mathbf{X}_{k})+n^{gnss} \tag{10}\]  
where \(n^{gnss}\) represents the Gaussian noise, \(h_{k}^{gnss}(\mathbf{X}_{k})\) represents the nonlinear measurement function and \(\mathbf{\mathcal{Z}}_{k}^{gnss}\) includes GNSS positioning \(\mathbf{p}_{k}^{G}\) and velocity \(\mathbf{v}_{k}^{G}\) information. The information obtained from GNSS is transformed into a world frame by the rotation matrix \(\mathbf{R}_{G}^{W}\). Thus, the GNSS factor node residual can be computed as follows:  
\[\mathbf{r}_{gnss}(\mathbf{Z}_{k}^{gnss},\mathbf{X}_{k})=\mathbf{Z}_{k}^{gnss}- h_{k}^{gnss}(\mathbf{X}_{k})=\begin{bmatrix}\mathbf{p}_{k}^{G}-\mathbf{p}_{k}^{W}\\ \mathbf{v}_{k}^{G}-\mathbf{v}_{k}^{W}\end{bmatrix} \tag{11}\]  
The data from LiDAR is typically calculated using the ICP method to obtain the relative pose information between adjacent moments [38; 39].  
\[\left(\mathbf{R}_{k}^{L},\mathbf{t}_{k}\right)^{*}=\text{arg min}\frac{1}{P_{k-1}}\sum_{i=1}^{P_{k-1}}\left\|P_{k}^{i}-\left(\mathbf{R}^{L} \cdot P_{k-1}^{i}+\mathbf{t}^{L}\right)\right\|^{2} \tag{12}\]  
where \(\mathbf{R}_{k}^{L}\) and \(\mathbf{t}_{k}^{L}\) represent the calculation results of rotation and translation of LiDAR point clouds \(P_{k-1}\) and \(P_{k}\) at time \(k\). \(\mathbf{R}^{L}\) and \(\mathbf{t}^{L}\) are the process variables. The optimal LiDAR measurements can be obtained through iterative solving, which can be defined as follows:  
\[\mathbf{Z}_{k}^{lidar}=h_{k}^{lidar}(\mathbf{X}_{k})+n^{lidar} \tag{13}\]Based on the factor graph function, it is assumed that the navigation parameters from LiDAR follow a Gaussian noise distribution, and LiDAR measurements are transformed into the world frame by \(\mathbf{R}_{L}^{W}\). The LiDAR factor node residual can be expressed as follows:  
\[\mathbf{r}_{lidar}(\mathbf{Z}_{k}^{lidar},\mathbf{X}_{k})=\begin{bmatrix} \mathbf{R}_{L}^{W}\mathbf{r}_{k}^{L}-\mathbf{P}_{k}^{W}\\ \mathbf{R}_{L}^{W}\frac{\partial\mathbf{r}_{k}^{L}}{\partial\mathbf{r}_{k-1,k }}-\mathbf{v}_{k}^{W}\end{bmatrix}=\begin{bmatrix}\mathbf{p}_{L}^{L}-\mathbf{ p}_{k}^{W}\\ \mathbf{v}_{k}^{L}-\mathbf{v}_{k}^{W}\end{bmatrix} \tag{14}\]  
where \(\mathbf{p}_{k}^{L}\) and \(\mathbf{v}_{k}^{L}\) represent the final positioning and velocity information of the LiDAR measurement. The rotation information obtained from LiDAR is used to calculate the transition matrix and to assist in IMU rotation correction. Based on the sensor measurement residuals, the MAP estimation of the whole system can be described as follows:  
\[\begin{split}\mathbf{X}_{k}^{*}=\underset{\mathbf{X}}{\text{arg min}}\left\{\left\|\mathbf{r}_{p}-h_{p}(\mathbf{X}_{k})\right\|_{\sum_{p}}^{2}+ \sum\limits_{m\in[k-N,k]}\left\|\mathbf{r}_{imu}(\mathbf{Z}_{B_{m+1}}^{B_{m}} \mathbf{X}_{m})\right\|_{\sum_{imu,m}}^{2}+\right.\\ \left.\sum\limits_{m\in[k-N,k]}\left\|\mathbf{r}_{gnss}(\mathbf{Z}_{m}^{ gnss},\mathbf{X}_{m})\right\|_{\sum_{gnss,m}}^{2}+\sum\limits_{m\in[k-N,k]} \left\|\mathbf{r}_{lidar}(\mathbf{Z}_{m}^{lidar},\mathbf{X}_{m})\right\|_{ \sum_{lidar,m}}^{2}\right\}\end{split} \tag{15}\]  
where \(\left\|\mathbf{r}_{p}-h_{p}(\mathbf{X}_{k})\right\|_{\sum_{p}}^{2}\) denotes the initialize and marginalize information and \(\sum_{p}\) represents the prior covariance. \(\sum_{imu,m}\), \(\sum_{gnss,m}\), and \(\sum_{lidar,m}\) represent the measurement covariances of IMU, GNSS and LiDAR at time \(m\), respectively, and \(N\) represents the width of the sliding window.