# An Improved Multi-Sensor Fusion Navigation Algorithm Based on the Factor Graph
## 4 Information Fusion Method Based on the Factor Graph
### Factor Graph Modeling
\right]^{T}g]\cdot \Delta t\);
* Attitude is calculated by \(\mathbf{q}_{i+1}=\mathbf{q}_{i}+[\frac{1}{2}\mathbf{q}_{i}\otimes(\widetilde{\mathbf{w}}_{ib}^ {b}-\
abla_{g}-\mathbf{\omega}_{g})]\cdot\Delta t\). * Add \(\mathbf{X}_{i+1}=\left[ \begin{array}{cc}\mathbf{p}_{i+1}^{T}&\mathbf{v}_{i+1}^{T}&\mathbf{q}_ {i+1}^{T}&\
abla_{a}^{T}&\
abla_{g}^{T}\end{array} \right]\) to \(\mathbf{X}_{new}=\{\}\);
* When the system receives the measurement \(\mathbf{Z}_{x,k}\) (magnetic, GPS, sonar or optic flow, etc.) at \(t_{k}\) moment, the factor node \(f_{x,k}\)(magnetic, GPS, sonar or optic flow, etc.) will be added into the graph. Add \(f_{x,k}\) to \(f_{new}=\{\}\). * The optimization problem encoded by the factor graph is solved by Gauss-Newton iterations. \(\mathbf{Z}\) is the set of all measurements, and \(\mathbf{X}\) represents the set of all variables. \(\mathbf{\hat{X}}\) is an initial estimate of \(\mathbf{X}\). According to Equation (17), the increment \(\Delta\mathbf{X}\) needs to be calculated, which should satisfy Equation (23). \[\operatorname*{argmin}_{\Delta\mathbf{X}}\left\|I(\mathbf{\hat{X}})\Delta\mathbf{X}-r( \mathbf{\hat{X}})\right\|^{2}\] (23) where \(J(\mathbf{\hat{X}})\) is the Jacobian matrix and \(r(\mathbf{\hat{X}})\) is the residual of all measurements. Calculating the increment \(\Delta\mathbf{X}\) requires factoring the Jacobian matrix into an equivalent upper triangular form, such as QR factorization. Once the increment \(\Delta\mathbf{X}\) is calculated, the new estimate \(\mathbf{\hat{X}}+\Delta\mathbf{X}\) can be obtained. It is set to be the initial estimate in the next iteration. For an example of the system, the factor graph in Figure 7a contains the following four factors which are expressed in the box. The system in Figure 7b adds a new factor, a block row is added in the Jacobian matrix, which is denoted as \(\overline{\times}\). The square root information matrix \(R\) in Figure 7a is as follows, which is obtained by QR factorization of the Jacobian matrix:  
\[R= \begin{bmatrix}\mathbf{X}_{1}&\mathbf{X}_{2}&\mathbf{X}_{3}\\ \begin{array}{cc}\times&\times&\\ &\times&\times\\ &&\times\end{array}\\ \end{bmatrix}  \tag{24}\]  
Figure 7: The factor graph and the associated Jacobian matrix in two moments. (**a**) The factor graph and the associated Jacobian matrix; (**b**) The factor graph and the associated Jacobian matrix when adding new factor node. When the new factor node is added in Figure 7b, the corresponding square root information matrix \(R^{\prime}\) can be changed as follows:  
\[R^{\prime}= \begin{bmatrix}\begin{array}{ccccc}\mathbf{X_{1}}&\mathbf{X_{2}}&\mathbf{X_{3}} &\mathbf{X_{4}}\\ \end{array}\\ \begin{array}{ccccc}\times&\times&&\\ &\times&\times\\ &&\overline{\times}&\overline{\times}\\ &&&\overline{\times}\end{array}\end{bmatrix}  \tag{25}\]  
The modified value, which is different from \(R\), is denoted as \(\overline{\times}\). It can be seen that the first two block rows remain unchanged. So, there is no need to recalculate these values. Step 6: Determine whether the new measurement is received by the system. Return to Step 2.