# 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.2 IS Algorithm  
The factor graph model intuitively manifests as the increasing number of variables in \(X\) over time. It is clearly unable to meet the real-time requirements of the system, if every factor graph inference involves re-solving the optimal estimate for all variables \(X\). Taking the factor graph model shown in Figure 3a as an example, variable elimination [36] is adopted and all nodes are eliminated from the factor graph by the order of \(x_{0}\rightarrow\alpha_{0}\to x_{1}\rightarrow\alpha_{1}\), as shown in Figure 4a. A Bayesian network is a directed acyclic graph that encodes the conditional densities of each variable. The Bayesian network is equivalent to the upper triangular matrix \(R\) resulting from the Jacobian matrix factorization \(J=QR\)[37], thereby representing the square root information matrix of the system. The corresponding Jacobian matrix \(J_{1_{1}}\) and the upper triangular matrix \(R_{t_{1}}\) at time \(t_{1}\), as shown in Figure 3a and obtained through a QR factorization of \(J_{t_{1}}=QR_{t_{1}}\), take the following form  
\[J_{t_{1}}= \begin{bmatrix}x_{0}&\alpha_{0}&x_{1}&\alpha_{1}\\ \times&\times&\times\\ &\times&&\\ &\times&&\times\\ &&\times&\end{bmatrix} \,\ R_{t_{1}}= \begin{bmatrix}\times&\times&\times\\ &\times&\times&\times\\ &\times&\times\\ &&\times&\end{bmatrix}  \tag{20}\]  
where \(\times\) represents non-zero elements in matrix entries \(J_{t_{1}}\) and \(R_{t_{1}}\). Each linearized factor is denoted by corresponding rows in the Jacobian matrix \(J_{t_{1}}\) and the upper triangular matrix \(R_{t_{1}}\) corresponds to the square root information matrix of the system. Figure 4: Factor graph model for asynchronous, delayed, and missing measurements. (**a**) GPS and VO measurement frequency are the same as the system output frequency; (**b**) GPS and VO measurement frequency lower than system output frequency, and the missing GPS measurement (measurement \(f_{2}^{GPS}\) is missing). As seen, part of the Bayesian network is changed when new nodes are added, assuming that the new measurement \(w\) and residual \(r\) are added at time \(t_{2}\). Compared to the factor graph at time \(t_{1}\), the new state variable node \(x_{2}\) and IMU bias nodes \(\alpha_{2}\) are added into the factor graph at time \(t_{2}\). Additionally, GPS, bias, and equivalent IMU factors are also introduced. The new factor graph is shown in Figure 3b, with the new added factors \(f_{2}^{Equiv}\) and \(f_{2}^{GPS}\). The updated Jacobian matrix is \(J_{t_{2}}=\left[ \begin{array}{c}{{{f_{t_{1}}}}}\\ {w}\end{array} \right]\) and the corresponding residual is \(b^{*}=\left[ \begin{array}{c}{b}\\ {r}\end{array} \right]\); thus, the upper triangular matrix \(R_{t_{2}}\) is given by  
\[R_{t_{2}}= \begin{bmatrix}Q^{T}&\\ &{I}\end{bmatrix}\begin{bmatrix}{{{f_{t_{1}}}}}\\ {w}\end{bmatrix} = \begin{bmatrix}{{{R_{t_{1}}}}}\\ {w}\end{bmatrix} \text{, }d^{*}= \begin{bmatrix}Q^{T}&\\ &{I}\end{bmatrix}\begin{bmatrix}{{b}}\\ {r}\end{bmatrix} = \begin{bmatrix}{{{d}}}\\ {r}\end{bmatrix}  \tag{21}\]  
where \(d^{*}\) is the residual term of the upper triangular matrix \(R_{t_{2}}\) after QR factorization of the Jacobian matrix \(J_{t_{2}}\). The updated Jacobian matrix \(J_{t_{2}}\) and the upper triangular matrix \(R_{t_{2}}\) can be shown as  
\[J_{t_{2}}=\left[ \begin{array}{cccccccc}x_{0}&\alpha_{0}&x_{1}&\alpha_{1}&x_{ 2}&\alpha_{2}\\ \times&\times&\times&&&&\\ &\times&&&&\\ &\times&&&\times&&&\\ &&\frac{\times}{\times}&\overline{\times}&\overline{\times}&\\ &&&\overline{\times}&\overline{\times}&\overline{\times}\\ &&&&\overline{\times}&\overline{\times}\end{array} \right],\,R_{t_{2}}=\left[ \begin{array}{cccccccc}x_{0}&\alpha_{0}&x_{1}&\alpha_{1}&x_{2}&\alpha_{2} \\ \times&\times&\times&&&\\ &&\times&\times&\times&&&\\ &&\overline{\times}&\overline{\times}&\overline{\times}&\overline{\times}\\ &&&&\overline{\times}&\overline{\times}&\overline{\times}\\ &&&&\overline{\times}&\overline{\times}\\ \end{array}