# An Adaptive Fast Incremental Smoothing Approach to INS/GPS/VO Factor Graph Inference
## 2 Materials and Methods
### Factor Graph Inference Algorithm for Integrated Navigation
#### 2.2.3 AFIS Algorithm  
From the analysis of incremental QR decomposition in Equation (21), non-zero terms emerge beneath the diagonal of the associated upper triangular matrix \(R\) upon the incorporation of new measurements into the integrated navigation system, necessitating local QR decomposition. However, the recently introduced measurement solely pertains to a subset of variables, influencing only the corresponding section of these variables in the upper triangular matrix (as shown in the red segment of the matrix \(R_{t_{2}}\) in Figure 5). Consequently, continuous trajectory smoothing is deemed unnecessary, suggesting a more judicious approach of optimizing the impacted variables directly during the resolution of the joint optimization equation (smooth when the system has requirement). This optimization retains the integrity of the system's matrix \(R\). The matrix R of the entire system proves instrumental in executing global optimization, especially with the introduction of a loop factor in the system (optimizes variables within the closed loop) or when a navigation task requires comprehensive optimization of all navigation variables at a specific juncture (optimizes all navigation variables). Treating the matrix \(R\) corresponding to the entire system as a network enables a swift execution of the optimization algorithm, employing a blend of adaptive local and global optimization. This amalgamation is aptly named the Adaptive Fast Incremental Smoothing (AFIS) algorithm, elucidated in the schematic diagram presented in Figure 7.  
At each moment, when new sensor measurements are added to the integrated navigation system, corresponding factor nodes and variable nodes need to be added to the factor graph model, which is equivalent to adding new rows to the linearized least squares equations of the system. Due to the fact that newly added sensors only affect their adjacent state variables, local QR decomposition can be used to update the changing parts of the square root mean information matrix R in the system. The state variables that generate changes can form an adaptive window, and only the state variables X' within that window are optimized and solved at each moment. Global state variables optimization is performed according to the system requirements. The system requirements here can be manually set, such as conducting global optimization every certain time or completing a certain maneuver task, depending on the computational requirements for algorithm complexity. The AFIS program flow chart is shown in Figure 8.  
Figure 6: Schematic diagram of IS (the changed variables are shown in red).  
It can be seen that the AFIS algorithm is based on the foundations of the IS algorithm, focusing on extracting the variable regions directly influenced by measurements for subsequent local optimization. Clearly, the AFIS algorithm yields a notable reduction in the computational complexity of optimization solutions, thereby enhancing the real-time performance of the system. This algorithm not only exhibits adaptability in selectively choosing and optimizing the network based on the added type of measurement but also accommodates closed-loop factors, enabling it to undertake global optimization in alignment with navigation requirements.