## 2 System Structures
### Factor Graph Structure  
The factor graph optimization approach has the characteristics of plug-and-play and historical information fusion. The ability of multi-source asynchronous data fusion makes factor graphs more reliable and robust than the EKF [33]. Therefore, the IMU/GNSS/LiDAR factor graph integrated system for vehicle navigation is constructed to estimate the navigation state in complex urban areas, which applies the ISAM2 sliding window algorithm to ensure real-time performance and accuracy. The structure of the factor graph system is shown in Figure 2.  
As shown in Figure 2, the system is estimated by fusing information from IMU pre-integration factors, LiDAR factors, and GNSS factors and introducing five types of factors to construct the whole framework. The information fusion frequency is based on the lowest frequency sensor, which is set as the GNSS frequency in this paper. The factor graph \(\text{G}=(\text{F}_{l},\text{X}_{m},\text{E}_{n})\) is a probabilistic graphical model (PGM) and mainly includes two kinds of nodes with edges connecting between them [34]. The factor node \(f_{i}\in\text{F}_{l}\) represents the measurement information from the sensors, and the variable node \(x_{j}\in\text{X}_{m}\) represents the state information to estimate the navigation state. The system is constructed by the factor nodes, with variable nodes connecting them by edges \(e_{ij}\in\text{E}_{n}\). The definition and the structure of the nodes are illustrated in Sections 2.3 and 2.4.  
Since the northeast up coordinate system is set as the navigation system, the system navigation state and the variable nodes at time \(k\) can be defined as follows:  
\[\text{X}_{k}=\left[\mathbf{p}_{k}^{\text{W}},\mathbf{v}_{k}^{\text{W}},\mathbf{ q}_{k}^{\text{W}},\mathbf{ba}_{k},\mathbf{bg}_{k}\right]^{\text{T}} \tag{1}\]  
where the first three parameters are information from each sensor measurement. \(\mathbf{p}_{k}^{\text{W}}\) includes the \(\mathbf{p}_{E},\mathbf{p}_{\text{N}},\mathbf{p}_{\text{U}}\) position results, \(\mathbf{v}_{k}^{\text{W}}\) includes the \(\mathbf{v}_{E},\mathbf{v}_{\text{N}},\mathbf{v}_{\text{U}}\) velocity results, and \(\mathbf{q}_{k}^{\text{W}}\) includes the \(\mathbf{q}_{E},\mathbf{q}_{\text{N}},\mathbf{q}_{\text{U}}\) platform rotation. \(\mathbf{ba}_{k}\) and \(\mathbf{bg}_{k}\) are accelerometer bias and gyroscope bias, respectively, including slowly changing bias and random noise bias.  
According to the state estimation and the factor graph mathematical model principle, the factor graph optimization method is typically derived as a nonlinear least-squares problem [35]. Assuming that the system model follows the Gaussian noise distributions, the factor edges can be defined as follows:  
Figure 2: Schematic diagram of factor graph optimization by the proposed IMU/GNSS/LiDAR integrated navigation system.  
\[f_{i}(\mathbf{X}_{k})\propto\exp(-\frac{1}{2}\left\|h_{k}^{i}(\mathbf{X}_{k})- \mathbf{Z}_{k}\right\|_{\Sigma_{i}}^{2}) \tag{2}\]  
where \(i\) indicates the type of sensor, \(h_{k}^{i}(.)\) represents the nonlinear measurement function, \(\mathbf{Z}_{k}\) represents the actual measurements of the sensors, and \(\sum_{i}\) represents the covariance matrix. The factor graph estimates the system state by solving the maximum a posteriori (MAP) probability of the probability distribution function (PDF) [36]. Through the transformation of the MAP formula, it can be defined as follows:  
\[\mathbf{X}_{k}^{*}=\text{argmin}\underset{I}{\sum}\left\|h_{k}^{i}(\mathbf{X} _{k})-\mathbf{Z}_{k}\right\|_{\sum_{i}}^{2} \tag{3}\]  
where \(\mathbf{X}_{k}^{*}\) represents the global optimal solution. To solve the nonlinear least squares problems, we generally apply the first-order Taylor expansion formula to linearize the function and use the Gaussian Newton method or Levenberg-Marquardt method.