## 3 Key Technology
### Multi-Stage Fault Detection, Isolation and Recovery Strategy  
The IMMFGO algorithm can adaptively adjust sensor weights and has specific fault detection capabilities. However, weight adjustment based on factor graph global optimization results makes it difficult to detect the impact of fault information on IMU compensation quickly. Therefore, this part proposes a multi-stage fault detection and isolation method, applying IMMFGO and the Chi-square FDI algorithm [41], which can effectively detect and reflect the impact of faulty sensors on the whole system. The Chi-square FDI algorithm detects faults for auxiliary sensors on the basis of IMU measurements. The overall process of MSFDIR is shown in Figure 4.  
Figure 4 shows that the system includes two stages: fault detection based on IMMFGO weight values and Chi-square fault detection based on IMU measurements. For Stage 1, the system detects faults based on the average and minimum weight values. When the average value is less than the threshold T\({}_{average}\) or the extreme minimum value is less than the threshold T\({}_{min}\), it is considered that the sensor accuracy is low in a period or there is a sudden interference. The fault detection function is defined as follows:  
\[\overline{p}_{k}^{n}=\frac{1}{N}*\sum_{k-N}^{k}\mu_{k}^{n} \tag{27}\]  
When there is no fault, the system is under the no-fault hypothesis H\({}_{0}\). When the system detects faults, the system is under the fault hypothesis H\({}_{1}\). Then, the detection function can be expressed as follows:  
\[\left\{ \begin{array}{l}\mathrm{H}_{0}:\forall n\in\{1,2,3,\ldots\},\overline {p}_{k}^{n}<\mathrm{T}_{average}\ \&\ \forall m\in\{k-N,k\},\mu_{m}^{n}<\mathrm{T}_{min}\\ \mathrm{H}_{1}:\exists n\in\{1,2,3,\ldots\},\overline{p}_{k}^{n}\geq\mathrm{ T}_{average}\ \parallel\exists n\in\{k-N,k\},\mu_{m}^{n}\geq\mathrm{T}_{min}\end{array} \right. \tag{28}\]  
Figure 4: Schematic diagram of the proposed MSFDIR algorithm. There are three auxiliary sensors, and the isolation and recovery functions are shown when a single sensor is faulty. When the first stage fault detection function works and the system is under hypothesis H1, the accuracy of IMMFGO will decrease and the faulty sensor needs to be isolated. Typically, the FDI algorithm based on IMU measurements is applied to evaluate the performance of assistant sensors [42], and the test statistic function of the residual chi-square value \(\lambda_{k}^{n}\) at time \(k\) can be defined as follows:  
\[\lambda_{k}^{n}=r_{k}^{n\text{T}}(S_{k}^{n})^{-1}r_{k}^{n} \tag{29}\]  
where \(r_{k}^{n}\) represents the residual between IMU measurements and assistant sensors. \(S_{k}^{n}\) represents the covariance matrix of the residuals. When the function detects no fault, the system is under the no-fault hypothesis H0; otherwise, the system is under hypothesis H1. Then, the detection function can be expressed as follows:  
\[\left\{ \begin{array}{l}\text{H}_{0}:\forall n\in\{1,2,3,\ldots\},\lambda_{k }^{n}<\text{T}_{D}^{n}\\ \text{H}_{1}:\exists n\in\{1,2,3,\ldots\},\ \lambda_{k}^{n}\geq\text{T}_{D}^{n} \end{array} \right. \tag{30}\]  
where T\({}_{D}^{n}\) is the second stage detection threshold related to the false alarm probability \(P_{fa}\) and standard deviation of sensors. When the system is under hypothesis H1, the fault sensors will be isolated and the framework will be reconstructed. When the isolated sensors perform well and the system is under hypothesis H1, the isolated sensor will be restored and the framework will be changed. The switching process of the integrated navigation system framework is shown in Figure 5.  
Figure 5 shows that the potential fault sensors will be isolated only when both stages are under hypothesis H1, and the isolated sensors will be restored only when both stages are under hypothesis H0. During the isolation process of the sensors, the IMMFGO algorithm can automatically switch the number of sub-models, while the isolated sensors still work for evaluation and restoration but do not provide navigation information.