# An Adaptive Fast Incremental Smoothing Approach to INS/GPS/VO Factor Graph Inference
###### Abstract  
In response to asynchronous and delayed sensors within multi-sensor integrated navigation systems, the computational complexity of joint optimization navigation solutions persistently rises. This paper introduces an adaptive fast integrated navigation algorithm for INS/GPS/VO based on factor graph. The factor graph model for INS/GPS/VO is developed subsequent to individual modeling of the Inertial Navigation System (INS), Global Positioning System (GPS), and Visual Odometer (VO) using the factor graph model approach. Additionally, an Adaptive Fast Incremental Smoothing (AFIS) factor graph optimization algorithm is proposed. The simulation results demonstrate that the factor-graph-based integrated navigation algorithm consistently yields high-precision navigation outcomes even amidst dynamic changes in sensor validity and the presence of asynchronous and delayed sensor measurements. Notably, the AFIS factor graph optimization algorithm significantly enhances real-time performance compared to traditional Incremental Smoothing (IF) algorithms, while maintaining comparable real-time accuracy. integrated navigation; factor graph; inertial navigation system; global positioning system; visual odometry +
Footnote †: journal: Medical  
of algorithmic design [12; 13; 14] and constraining the potential utilization of more effective sensor information to enhance navigation performance. Additionally, KF generally confines its estimation to the current state at the present time, and the formulated measurement equation solely delineates the mapping relationship between measurement information and the current state. Complexities arise in accommodating measurement information, such as VO, which characterizes constraint relationships between diverse states at different times and proves challenging to integrate within the information fusion framework based on KF. The factor graph represents a category of probability graph models tasked with encoding the probability relationships between navigation state variables and measurements obtained from navigation systems. The state variables are denoted as variable nodes, while each sensor measurement is identified as a factor node. The integration of measurements from diverse sensors, potentially operating asynchronously, necessitates the establishment of connections between the factors derived from these measurements and the corresponding nodes within the factor graph [15; 16]. The factor graph model exhibits notable flexibility, enabling a plug-and-play functionality for sensors, a feature that has garnered considerable attention in the field of navigation [17; 18; 19]. In contrast to state estimation methods in filter-based integrated navigation systems, factor-graph-based integrated navigation system state estimation methods abstain from marginalizing historical states. Instead, they undertake the estimation of all states within the integrated navigation system, a task akin to solving a nonlinear least squares problem. Batch optimization is extensively employed for information fusion in integrated navigation systems [20; 21; 22]. However, as batch optimization recalculates all variables of the nonlinear least squares problem, computational complexity escalates over time. In reference [23], the fixed-lag smoothing algorithm is implemented to address this issue by maintaining several past states in a sliding window. To sustain a consistent and manageable computational cost, all variables within the sliding window undergo recalculation upon obtaining a new measurement. This operation, however, proves unnecessary since some states remain unchanged. The fixed-lag smoothing algorithm is deemed sub-optimal in comparison to batch optimization unless the smooth lag is sufficiently extensive. Furthermore, the fixed-lag smoother marginalizes variables outside of the smoothing lag, posing challenges for the navigation system in establishing effective closed-loop factors. This compromises the outstanding performance of factor-graph-based integrated navigation.