# An Improved Multi-Sensor Fusion Navigation Algorithm Based on the Factor Graph
## 4 Information Fusion Method Based on the Factor Graph
### Factor Graph Modeling  
Not all of the sensors can be updated at the same time. In general, the frequencies of the IMU measurements are very high, which are different from other sensors. For example, the update frequency of GPS measurement is slower than IMU, which can be described in Figure 4.  
Variable node \(\mathbf{X}_{i}\) represents the state variable of the system. \(f_{x,i}(IMU)\) denotes the updating of the variable node with the IMU measurement \(P_{x,i}(IMU)\). The factor IMU is defined as the following form  
\[f_{x,i}(IMU)=p(\mathbf{X}_{i+1}|\mathbf{X}_{i}) \tag{21}\]  
It connects two different variable nodes \(\mathbf{X}_{i}\) and \(\mathbf{X}_{i+1}\) in \(t_{i}\) and \(t_{i+1}\) moments, respectively. The IMU measurement is used to calculate the state transition matrix to predict \(\hat{\mathbf{X}}_{i+1/i}\) by state vector propagation. Figure 4: The factor graph containing the GPS measurement. Figure 3: Factor graph representations of the state variable and the measurement. On the other hand, \(f_{x,i}(GPS)\) denotes the factor node when the system receives the measurement \(\mathbf{Z}_{x,i}(GPS)\). The factor GPS is defined as the following form  
\[f_{x,i}(GPS)=p(\mathbf{Z}_{i}^{GPS}|\mathbf{X}_{i}) \tag{22}\]  
The estimation of \(\mathbf{X}_{k}\) can be solved by using the method of seeking extreme through the probability density function \(p(\mathbf{Z}_{i}^{GPS}|\mathbf{X}_{k})\). It can be calculated as Equations (16) and (17). With this model, we can calculate the minimization of the local functions \(f(\mathbf{Z}_{i}|\mathbf{X}_{i})\) and \(f(\mathbf{X}_{i+1}|\mathbf{X}_{i})\) to acquire the optimal state at \(t_{i}\) moment. When the system receives the magnetic measurement \(\mathbf{Z}_{x,i+1}(\textit{Magnetic sensor})\), the factor node \(f_{x,i+1}(\textit{Magnetic sensor})\) will be added into the graph. Because the update frequency of the magnetic sensor is faster than that of GPS, the magnetic sensor nodes appear many times during the update of GPS. It is described in Figure 5.  
The update frequencies of different sensors in the MUAV are different. In a similar way, the factor graph model describes the characteristics of the MUAV navigation system, which are shown in Figure 6.  
This model enables us to handle situations where different sensors provide measurements at different times and at different time intervals. When the measurement equation of the new sensor is nonlinear, the method can simply add new factors to the graph. If a sensor becomes unavailable due to various reasons, the system simply stops adding the associated factors to the graph. There is no need for a special procedure. To implement the factor graph method, the steps are introduced as follows:  
* Set the initial parameters and define a state-space vector. New factors \(f_{new}=\{\}\) and new variables \(\mathbf{X}_{new}=\{\}\) are initialized. The probability density function \(p(\mathbf{X}_{0})\) should be set up according to the parameters of the system. * When the system receives the IMU measurement \(\tilde{\mathbf{f}}^{b}\), \(\tilde{\mathbf{w}}_{ib}^{b}\), at \(t_{i}\) moment, the factor node \(P_{x,i}(IMU)\) will be added into the graph. It connects two different variable nodes \(\mathbf{X}_{i}\) and \(\mathbf{X}_{i+1}\) in \(t_{i}\) and \(t_{i+1}\) moments, respectively. The IMU measurement is used to calculate the state transition matrix to predict \(\mathbf{\hat{X}}_{i+1/i}\) by state vector propagation. Figure 5: The factor graph containing GPS and magnetic measurement. Figure 6: The multi-sensor fusion framework based on factor graph. * Position is calculated by \(\mathbf{p}_{i+1}=\mathbf{p}_{i}+\mathbf{v}_{i}\cdot\Delta t\);
* Velocity is calculated by \(\mathbf{v}_{i+1}=\mathbf{v}_{i}+[\mathbf{C}_{\mathbf{v}}^{b}\cdot(\mathbf{\hat{p}}^{b}-\
abla_{a}- \mathbf{\omega}_{a})+\left[ \begin{array}{cc}0&0&1\end{array}