# 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.1 Factor Graph Algorithm  
In the factor graph model, the set of variable nodes is represented as \(X\); The set of factor nodes is represented as \(F\); The set of all edges of connecting nodes is represented as \(E\). Thus, the factor graph can be represented as  
\[G=(X,F,E) \tag{14}\]Based on the theory of factor graphs, the factor graphs \(G\) describe the factorization of functions \(f(X)\), which can be denoted as  
\[f(X)=\prod_{i}f_{i}(X_{i}) \tag{15}\]  
where \(X_{i}\) represents the set of variable nodes connected to factor nodes \(f_{i}\), \(X_{i}\subseteq X\).  
The problem needs to be solved in the factor graph inference algorithm to find the optimal estimate value \(\hat{X}\) of \(X\), so that the function \(f(X)\) takes the maximum value. The Maximum A Posteriori (MAP) estimate is expressed as  
\[\hat{X}=\underset{X}{\operatorname{argmax}}\left(\prod_{i}f_{i}(X_{i})\right) \tag{16}\]  
For the factors defined in Equations (5)-(7), (9), (11), and (13), assuming a Gaussian noise model, the cost function can usually be defined as \(d(a)\triangleq\exp\left(-\frac{1}{2}\|a\|_{\Sigma}^{2}\right)\). Here, \(\|a\|_{\Sigma}^{2}\doteq a^{\top}\Sigma^{-1}a\) represents the squared of the Mahalanobis distance, \(\Sigma\) represents the covariance matrix. Then, the factor \(f_{i}\) assumes the following form  
\[f_{i}(X_{i})\propto\exp\left(\frac{1}{2}\|a_{i}\|_{\Sigma_{i}}^{2}\right) \tag{17}\]  
Therefore, taking the maximum value of the above equation is equivalent to the least squares problem given by  
\[\underset{X}{\operatorname{argmin}}(-\log f(X))=\underset{X}{\operatorname{ argmin}}\frac{1}{2}\underset{i}{\sum}\|a_{i}\|_{\Sigma_{i}}^{2} \tag{18}\]  
The expression of factor nodes in factor graph models is usually nonlinear and can be linearized through Taylor expansion. When the initial estimation \(\hat{X}_{k}\) of the system state \(X_{k}\) is given, the update increment \(\Delta\) of state \(X_{k}\) can be transformed into solving a standard least squares solution  
\[\Delta^{*}=\underset{\Delta}{\operatorname{argmin}}(-\log f(\Delta))= \underset{\Delta}{\operatorname{argmin}}\left\|J\big{(}\hat{X}_{k}\big{)} \Delta-b\big{(}\hat{X}_{k}\big{)}\right\|^{2} \tag{19}\]  
where \(\Delta=\{\delta X_{k}\}\) represents the update increment for the current system variable \(X_{k}\), \(J\big{(}\hat{X}_{k}\big{)}\) and \(b\big{(}\hat{X}_{k}\big{)}\) are the Jacobian matrices and residuals of the cost functions at \(X_{k}\); then, the Gaussian Newton iteration method is used to calculate the optimal estimation value \(\hat{X}_{k}\).  
When the GPS and VO measurement frequency are different from the system output frequency and there is a time delay, it is only necessary to define the GPS or VO measurement values at time \(t_{k}\) as factor nodes and connect them to the INS factor graph model. If GPS measurements are missing, the corresponding GPS factor node is not connected to the INS factor graph model at time \(t_{k}\). Figure 4b illustrates the INS/GPS/VO factor model, reflecting the lower frequencies of GPS and VO measurements compared to the system output frequency, and the occurrence of missing GPS measurements.  
The process of constructing the factor graph model for the integrated navigation system described above reveals that the factor nodes corresponding to each sensor are independent and do not affect each other in the factor graph model. For instance, in the case of GPS effectiveness change, if GPS fails suddenly, we simply suppress the addition of GPS factor nodes and transition to an INS/VO factor graph model. Conversely, when GPS becomes effective, adding GPS factor nodes reinstates the INS/GPS/VO factor graph model. The use of factor graph models to describe integrated navigation problems can flexibly handle dynamic changes in sensor effectiveness, and the integrated navigation algorithm based on factor graphs has strong scalability and flexibility.