# An Improved Multi-Sensor Fusion Navigation Algorithm Based on the Factor Graph
## 5 Experiment and Discussion
### MUAV Flight Test and Results  
Outdoor autonomous flight tests on real data have been carried out in a playground. All of the sensors described in Section 2 are mounted on the vehicle. To evaluate the navigation performance, a more precise navigation system which has a differential GPS/INS system on the autopilot board is used as the reference system. For the multi-sensor navigation, GPS positioning is used in single point mode. We make use of the ARM processor with highly optimized C language to ensure real-time performance. The data are captured from the autopilot board of the MUAV during the flight. The total flight time was about 500 s.  
The MUAV system in the test flight experiment is shown in Figure 9(a). Autonomous flights include takeoff, waypoint, returning, and landing modes. The horizontal flight track compared with the expected route is presented in Figure 9(b). System dynamic parameters are shown in Table 3.  
\begin{table}
\begin{tabular}{c c c c c c c} \hline \hline  & \multicolumn{3}{c}{**Average RMSE in the Position**} & \multicolumn{3}{c}{**Average RMSE in the Velocity Error**} \\
**Error Type** & \multicolumn{3}{c}{**Error (units: m)**} & \multicolumn{3}{c}{**(units: m/s)**} \\ \cline{2-7}  & **Longitude** & **Latitude** & **Height** & **Eastern** & **Northern** & **Vertical** \\ \hline Extend Kalman filter & 1.212 & 1.205 & 0.703 & 0.141 & 0.142 & 0.049 \\ \hline Factor graph filter & 1.043 & 1.035 & 0.628 & 0.121 & 0.115 & 0.034 \\ \hline \hline \end{tabular}
\end{table}
Table 2: Statistical error contrast between two schemes.  
Figure 10: Square shaped outdoor trajectory in the flight experiment of the MUAV. (**a**) Test flight experiment of the MUAV; (**b**) Square shaped trajectory.  
The expected route with four waypoints is the pre-set track which is uploaded in the MUAV system before taking off. The circles of the four corners in the square represent the waypoints, and the default waypoint radius is configured to 3 m. This means that if the vehicle reaches the waypoint within the default waypoint radius, it will turn to the next waypoint. The flight track is obtained by the data of the MUAV navigation system. It can be seen that the vehicle can fly along the expected route. Figure 11a,b illustrates the performance of the position error and the velocity error generated by these two filters.  
\begin{table}
\begin{tabular}{c c c} \hline \hline
**Type** & **Parameters Item** & **Unit** \\ \hline Machine size & 608 \(\times\) 608 \(\times\) 243 & mm \\ Takeoff weight & 950 & g \\ Maximum payload & \(<\)580 & g \\ Flight time & 15 & min \\ \hline \hline \end{tabular}
\end{table}
Table 3: System dynamic parameters.  
Figure 11: Comparison curves of two filter methods. (**a**) Position error contrast curves. (**b**) Velocity error contrast curves.  
The position and velocity errors of the factor graph filter and EKF are compared in Table 4. It can be seen that the factor graph method has higher precision than EKF. The RMSE results reduce to less than 80%. Better results of the system by using the factor graph filter method can be obtained, and the accuracy level can meet the requirements of the MUAV navigation system.