# An Adaptive Fast Incremental Smoothing Approach to INS/GPS/VO Factor Graph Inference
## 3 Results
### AFIS Algorithm Analysis  
The section primarily examines the accuracy and real-time performance of the adaptive fast incremental smoothing factor-graph-based (AFIS-FG) algorithm, batch optimization factor-graph-based (Batch-FG) algorithm, fixed-lag smoothing factor-graph-based (FixLag-FG) algorithm, and incremental smoothing (IS-FG) algorithm. The IS-FG algorithm and our proposed AFIS-FG algorithm are shown in Sections 2.2.2 and 2.2.3, respectively. The Batch-FG algorithm recalculates all state variables of the nonlinear least squares problem, which is shown in references [20; 21; 22]. The FixLag-FG algorithm [23] maintains several past states in a sliding window, with all variables within the sliding window undergoing recalculation upon obtaining a new measurement, while FixLag-FG (5) means the length of the sliding window is 5. The simulation aims at the INS/GPS/VO integrated navigation in scenario 1 and scenario 2. The simulation results are shown in Figures 11 and 12, Tables 2 and 3. \begin{table}
\begin{tabular}{c c c c c c} \hline \hline
**Integrated** & \multicolumn{2}{c}{**Without Delay**} & \multicolumn{1}{c}{**dt = 0.1 s**} & \multicolumn{1}{c}{**dt = 0.3 s**} & \multicolumn{1}{c}{**dt = 0.5 s**} \\
**Navigation Method** & **KF** & **FG** & **FG** & **FG** & **FG** \\ \hline Radial Error (m) & 18.32 & 14.27 & 14.57 & 15.17 & 15.81 \\ \hline \hline \end{tabular}
\end{table} 
Table 1: RMSE statistical result of KF and FG in different measurement delays. Figure 10: Position error of KF and FG in different measurement delays. Figure 11: Radial position error of different algorithms in different scenarios. (**a**) Position error in scenario 1; (**b**) Position error in scenario 2. Figure 12: Comparison of total runtime of different algorithms in different scenarios. (**a**) Total runtime in scenario 1; (**b**) Total runtime in scenario 2. From Figure 12 and Table 3, it is evident that FixLag (5) exhibits the shortest optimization time, clocking in at 0.2665 s in scenario 1 and 0. 3971 s in scenario 2. This efficiency arises from its exclusive focus on optimizing and updating local state variables within the smoothing window at each moment. In contrast, the optimization times of Batch-FG, IS-FG, and AFIS-FG gradually escalate as simulation time progresses. Batch-FG stands out with the lengthiest optimization time, recorded at 54.6 in scenario 1 and 257.1 s in scenario 2. This is attributed to Batch-FG's overarching approach, involving unified processing of all received measurements at all times. This approach is akin to batch processing, entailing the update of the system's upper triangular matrix \(R\) at each time, followed by global optimization. IS-FG secures the second shortest optimization time, totaling 7.4 s in scenario 1 and 32.5 s in scenario 2. This is a consequence of IS-FG's incremental update of the upper triangular matrix \(R\) of the system at each moment, a departure from the Batch-FG's approach. AFIS-FG follows with the third shortest optimization time, amounting to 4.9 s in scenario 1 and 20.9 s in scenario 2. AFIS-FG employs incremental updates to the upper triangular matrix of the system upon receiving measurements, focusing its optimization on locally affected state variables. Additionally, global optimization is executed by retaining the upper triangular matrix in the presence of a loop factor in the factor graph. The precision analysis from Figure 11 and Table 2 reveals that the accuracy achieved by IS-FG and AFIS-FG is comparable to that of Batch-FG, surpassing the performance of FixLag-FG (5). This discrepancy arises due to the distinct optimization strategies employed by these algorithms. Specifically, Batch-FG, IS-FG, and AFIS-FG optimize global variables, whereas FixLag-FG (5) exclusively focuses on variables within a smoothing window. Notably, during instances of a loop factor in the system (as evident in the 310 s segment in Figure 11b), Batch-FG, IS-FG, and AFIS-FG leverage this loop factor to rectify the current time state, thereby enhancing real-time navigation accuracy.