## 4 Experimental Analysis
### Experimental Results of MSFDIR  
In Part 4.2, we analyzed and validated the performance of the IMMFGO algorithm through vehicle experiments. Based on the work above, we will further analyze the impact of fault detection, isolation, and recovery functions.  
Based on the MSFDIR algorithm proposed in Part 3.2, the first stage average and minimum fault detection threshold based on IMMFGO is set to 0.05 and 0.02, and the stage 2 fault detection threshold is set as five times the standard deviation of sensors. The recovery threshold is set as the same. Figure 10 shows the results of the fault detection functions and the usage of sensors.  
From Figure 10, we can see that in road section (1), when there is a slowly changing error, the stage 1 detection function can respond to the error, and the stage 2 detection function can detect errors that have a significant impact on the IMU and isolate the sensor. In road section (2), when there is a stable error, the IMMFGO algorithm can automatically optimize sensor weights, thus avoiding significant errors. In road section (3), when there are  
Figure 10: Results of MSFDIR in vehicle experiments. (**a**) is the result of the stage 1 IMMFGO fault detection function. (**b,c**) are the results of the stage 2 GNSS and LiDAR fault detection function based on IMU, respectively. (**d**) is the usage result of GNSS and LiDAR.  
step faults, the stage 1 and stage 2 detection functions can identify errors that significantly impact position accuracy and isolate the corresponding sensors. In other road sections, gradual and step faults that have a significant impact on system performance can also be detected and isolated to ensure the performance of the IMMFGO algorithm and improve system accuracy. The position error results for the east, north, and vertical directions of the proposed IMMFGO and MSFDIR algorithm are shown in Figure 11a. The box diagram of position errors also shows the superiority of the proposed method in Figure 11b.  
Figure 11a shows that when a significant fault occurs, the factor graph optimization algorithm has the characteristic of global optimization based on historical information, which will generate cumulative errors. The MSFDIR algorithm can detect and reduce the impact of significant or slowly changing faults, thus improving the performance of system state estimation based on the IMMFGO algorithm. The RMSE of several factor graph optimization algorithms for quantitative analysis is shown in Table 4.  
Figure 11b and Table 4 show that after adopting the MSFDIR function, the positioning accuracy is improved and the 3D position error is decreased to 4.42 m. Compared to the IMMFGO algorithm, the accuracy has improved by 9.4%, and the positioning accuracy in the east, north, and vertical directions has improved. The proposed MSFDIR function can obtain the usage information of sensors to provide more information for vehicle navigation systems to ensure system security. The MSFDIR function can compensate for the problem of slowly increasing or significant faults leading to a decrease in the positioning accuracy of the IMMFGO algorithm. Meanwhile, MSFDIR also provides a reference for fault warnings in vehicle navigation systems. At the same time, the real-time performance of MSFDIR can also meet the actual needs of vehicle navigation systems.  
\begin{table}
\begin{tabular}{c c c c c c c} \hline \hline
**Types of Error** & **EKF** & **FGO** & **AFGO** & **IMMFGO** & **MSFDIR** & **Improvement** \\ \hline East & 9.07 & 8.16 & 3.66 & 2.85 & 2.64 & 7.4\% \\ North & 10.05 & 3.96 & 3.95 & 2.90 & 2.66 & 8.3\% \\ Vertical & 5.87 & 4.62 & 3.46 & 2.72 & 2.35 & 13.6\% \\
3D & 14.14 & 10.18 & 6.4 & 4.88 & 4.42 & 9.4\% \\ \hline \hline \end{tabular}
\end{table}
Table 4: RMSE of five algorithms and the improvement from MSFDIR to IMMFGO (unit: meters).  
Figure 11: (**a**) Comparison results of the position errors in the east, north, and vertical directions between the proposed IMMFGO and MSFDIR. (**b**) The box diagram of position errors.