# An Adaptive Fast Incremental Smoothing Approach to INS/GPS/VO Factor Graph Inference
## 2 Materials and Methods  
### Factor Graph Model of INS/GPS/VO Integrated Navigation System  
#### 2.1.1 INS Factor Graph Model  
The efficacy of INS heavily relies on the high-frequency measurement of carrier motion states facilitated by the Inertial Measurement Unit (IMU). This frequency is typically considerably higher than the output frequency of the navigation system. Consequently, the INS measurement data contain a substantial amount of redundant information compared to the navigation system output. Therefore, the idea of equivalent factors can be adopted [28] when establishing the INS factor graph model. State variables requiring output are introduced as variable nodes. The IMU measurement data between consecutive time points of adjacent variable nodes is integrated and defined as equivalent factor nodes. This approach serves to reduce the update frequency of factor nodes, thereby enhancing the real-time performance of the algorithm. Figure 1 shows the construction scheme of the INS factor graph model. First, the output frequency of navigation state variables is determined based on the actual system application requirements. Subsequently, the update period for equivalent factors is selected. Navigation state variables earmarked for output, identified as target variables, are incorporated as variable nodes into the factor graph. Additionally, the inertial sensor bias variable is introduced to the variable node to rectify the measurement error of the inertial sensor in IMU. The bias factor node is defined using the error propagation function. During an update period, the IMU measurement data is utilized to establish a pre-integration factor node, connecting it to the corresponding variable nodes in the factor graph. The INS functions as a dead reckoning system, amalgamating measurements of acceleration and angular velocity to derive information regarding the position, velocity, and attitude of the carrier. Utilizing the pre-integration method enables the synthesis of IMU data within an update cycle, establishing equivalent factor nodes. The integrated navigation system leverages auxiliary navigation methods to swiftly accomplish the initial alignment  
Figure 1: Construct scheme of INS factor graph model. of the INS, facilitating the acquisition of initial position, velocity, and attitude information of the carrier. Subsequently, by directly pre-integrating acceleration and angular velocity measurements within the body frame, significant reductions in computational complexity and improvements in the real-time performance of the algorithm are achieved [29]. The output time of the navigation state is denoted by \(t_{k}\), \(k=0,1,\dots\), and consequently, the equivalent factor update period will be \(T=t_{k+1}-t_{k}\). By pre-integrating IMU measurement data within an update period \(t_{k}\sim t_{k+1}\) under the body frame, the calculation formula for position, velocity, and attitude increments is obtained as  
\[\left\{ \begin{array}{l}\Delta p_{t_{k+1}}^{t_{k}}=\iint_{t_{k}}^{t_{k+1}} \Bigl{(}C_{bt}^{bt_{k}}\Bigl{(}f_{t}^{b}-\
abla_{f}\Bigr{)}\Bigr{)}dt^{2}\\ \Delta v_{t_{k+1}}^{t_{k}}=\int_{t_{k}}^{t_{k+1}}\Bigl{(}C_{bt}^{bt_{k}}\Bigl{(} f_{t}^{b}-\
abla_{f}\Bigr{)}\Bigr{)}dt\\ \Delta\theta_{t_{k+1}}^{t_{k}}=\int_{t_{k}}^{t_{k+1}}\Bigl{(}E_{bt}^{bt_{k}} \Bigl{(}\omega_{t}^{b}-\
abla_{\omega}\Bigr{)}\Bigr{)}dt\end{array} \right. \tag{1}\]  
where \(C_{bt}^{bt_{k}}\) and \(E_{bt}^{bt_{k}}\) are the rotation matrix and rotation rate matrix from the body frame of current time to the body frame of time \(t_{k}\); \(f_{t}^{b}\) and \(\omega_{t}^{b}\) are the specific force and the angular acceleration in the body frame; \(\
abla_{f}\) and \(\
abla_{\omega}\) are the bias variables of the accelerometer and gyroscope. According to the principle of INS dead reckoning, the carrier's position, velocity, and attitude at the time \(t_{k+1}\) can be further obtained as  
\[\left\{