# 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
\begin{array}{l}p_{t_{k+1}}^{n}=p_{t_{k}}^{n}+(t_{k+1}-t_{k})\sigma_{ t_{k}}^{n}+\frac{1}{2}(t_{k+1}-t_{k})^{2}g^{n}+C_{bt_{k}}^{n}\Delta p_{t_{k+1}}^{ t_{k}}\\ v_{t_{k+1}}^{n}=v_{t_{k}}^{n}+(t_{k+1}-t_{k})g^{n}+C_{bt_{k}}^{n}\Delta v_{t_{k+1 }}^{t_{k}}\\ \phi_{t_{k+1}}^{n}=\phi_{t_{k}}^{n}+C_{bt_{k}}^{n}\Delta\phi_{t_{k+1}}^{t}\end{array} \right. \tag{2}\]  
where \(g^{n}\) is gravity vector. Equation (2) is the discrete form of the INS system state equation, which can be uniformly expressed as  
\[ \begin{array}{l}\hat{x}_{k+1}=h\bigl{(}x_{k},\alpha_{k},z_{k}^{IMU}\bigr{)} \\ \hat{x}_{k+1}=g(\alpha_{k})\end{array}  \tag{3}\]  
where \(x_{k}\) represents the navigation state variable, including position, velocity, and attitude angle; \(\alpha_{k}\) represents the bias variable of the inertial sensor; \(z_{k}^{IMU}\) represents the measured value of the inertial sensor; \(\hat{x}_{k+1}\) is the predicted value of the navigation state value obtained from the state equation at time \(t_{k+1}\); Conceptually, each equation in (3) defines distinct factors: an IMU factor \(f^{IMU}\) linking the navigation nodes \(x_{k}\), \(\hat{x}_{k+1}\) and the bias node \(\alpha_{k}\), alongside a bias factor \(f^{Bias}\) connecting the bias nodes \(\alpha_{k}\) and \(\hat{x}_{k+1}\). In practice, consecutive IMU measurements are amalgamated into a singular factor, as depicted in Figure 2. Nonetheless, delineating explicit expressions for the conventional IMU factor \(f^{IMU}\) remains beneficial. The conventional IMU factor \(f^{IMU}\) corresponding to a specific IMU measurement \(z_{k}^{IMU}\) is defined as follows. The IMU measurement \(z_{k}^{IMU}\) and the current estimate of the state \(x_{k}\), \(\alpha_{k}\) are utilized to forecast the values for the state \(\hat{x}_{k+1}\). The discrepancy between this  
Figure 2: The comparison between IMU factor and equivalent IMU factor. (**a**) Factor graph model using IMU factor; (**b**) Factor graph model using equivalent IMU factor. prediction and the current estimate \(x_{k+1}\) is encapsulated as the error function represented by the factor  
\[f_{k+1}^{IMU}(x_{k+1},x_{k},\alpha_{k})\doteq d(x_{k+1}-h(x_{k},\alpha_{k},z_{k})) \tag{4}\]  
where the operator \(d(\cdot)\) denotes a specific cost function. Upon the addition of the new variable node \(x_{k+1}\) to the graph, an appropriate reasonable initial value for \(x_{k+1}\) is necessary. This value can be derived, for instance, from the prediction \(h(\hat{x}_{k},\hat{x}_{k},z_{k})\), where \(\hat{x}_{k}\) and \(\hat{x}_{k}\) represent the most recent estimates of the state \(x_{k}\) and the measurement \(\alpha_{k}\), respectively. Similarly, the bias factor associated with the modeled IMU errors is expressed as  
\[f_{k+1}^{Bins}(\alpha_{k+1},\alpha_{k})\doteq d(\alpha_{k+1}-g(\alpha_{k})) \tag{5}\]  
with \(\alpha_{k+1}\) and \(\alpha_{k}\) represented as variable nodes in the factor graph. Due to the high frequency of inertial measurement data (generally greater than 200 Hz), the inertial system has a high calculation frequency, which means that the number of variable nodes and corresponding cost functions in the navigation system factor graph model increase sharply, leading to a sharp increase in the computational complexity of joint optimization. Instead, consecutive IMU measurements can be combined into an equivalent IMU factor [29], relating between two distant navigation and calibration nodes, thereby avoiding introducing new variables into the optimization at IMU rate. The equivalent IMU factor is given by  
\[f^{Equiv}\big{(}x_{j},x_{i},\alpha_{k}\big{)}\doteq d\Big{(}x_{j}-h^{Equiv} \big{(}x_{i},\alpha_{i},\Delta x_{i\to j}\big{)}\Big{)} \tag{6}\]  
where \(\Delta x_{i\to j}\) relates between the navigation states at the two time instances \(x_{i}\) and \(x_{j}\), comprising the accumulated change in position, velocity, and attitude. \(h^{Equiv}\) represents the nonlinear function that predicts \(x_{j}\) given \(x_{i}\), \(\alpha_{i}\) and \(\Delta x_{i\to j}\).