# Improved Adaptive Federated Kalman Filtering for INS/GNSS/VNS Integrated Navigation Algorithm
## 3 Improved Adaptive Federated Kalman Filter (IAFKF)
### Fault Detection  
According to the Kalman filter equation, the recursive value at time \(k\) is derived from the previous time \(k-1\):  
\[\hat{\mathbf{z}}_{i;k/k-1}=\mathbf{H}_{i}\hat{\mathbf{x}}_{i;k/k-1} \tag{21}\]  
The measurement residual is as follows [26]: \(\mathbf{r}_{i;k}=(\mathbf{z}_{i;k}-\hat{\mathbf{z}}_{i;k/k-1})\)  
In this equation, \(\mathbf{r}_{i;k}\) represents the measurement residual of the \(i\)-th sub-filter at time \(k\) in distributed filtering. When there is no fault in the system, the residual \(\mathbf{r}_{i;k}\) follows a Gaussian white noise distribution with a mean of 0, and its covariance matrix is as follows:  
\[\begin{split}\mathbf{w}_{i;k}&=E(\mathbf{r}_{i;k}\mathbf{r}_{ i;k}^{T})=E\Big{\{}[\mathbf{H}_{i;k}\cdot(\mathbf{x}_{i;k}-\hat{\mathbf{x}}_{i;k/k-1})+\mathbf{v}_{ i;k}]\cdot[\mathbf{H}_{i;k}\cdot(\mathbf{x}_{i;k}-\hat{\mathbf{x}}_{i;k/k-1})+\mathbf{v}_{i;k}]^{T} \Big{\}}\\ &=E[(\mathbf{H}_{i;k}\cdot(\mathbf{x}_{i;k}-\hat{\mathbf{x}}_{i;k/k-1}))(\bm {x}_{i;k}-\hat{\mathbf{x}}_{i;k/k-1})^{T}\cdot\mathbf{H}_{i;k}^{T}]+E(\mathbf{v}_{i;k}\mathbf{ v}_{i;k}^{T})\\ &=\mathbf{H}_{i;k}\mathbf{P}_{i;k/k-1}\mathbf{H}_{i;k}^{T}+\mathbf{R}_{i;k}\end{split} \tag{22}\]  
A fault detection statistic based on the characteristics of the chi-squared distribution can be constructed as follows:  
\[\lambda_{i;k}=\mathbf{r}_{i;k}^{T}\mathbf{w}_{i;k}^{-1}\mathbf{r}_{i;k} \tag{23}\]  
In this equation, \(\lambda_{i;k}\sim\chi^{2}(m)\) follows a chi-squared distribution with \(m\) degrees of freedom (the dimension of the measurement \(\mathbf{Z}\)). If a fault occurs, the residual \(\lambda_{i;k}\) will no longer be a zero-mean white noise process, and therefore the following method can be used to detect faults.  
The fault detection criteria are as follows:  
\[\begin{cases}\lambda_{i;k}\geq T_{d},&C_{k}=0\\ \lambda_{i;k}<T_{d},&C_{k}=1\end{cases} \tag{24}\]  
In this context, \(T_{d}\) represents the fault detection threshold, and \(C_{k}\) represents the fault flag at time \(k\). When \(C_{k}=0\), the measurement system has a fault and requires isolation processing. When \(C_{k}=1\), the measurement system is functioning normally. The value of \(T_{d}\) can be calculated based on Equation (25) and the degree of freedom \(m\) and confidence level using a chi-squared distribution table:  
\[P=\int_{T_{d}}^{\infty}f\chi_{(m)}^{2}(x)dx=1-\int_{0}^{T_{d}}f\chi_{(m)}^{2} (x)dx=\alpha \tag{25}\]  
To eliminate occasional abnormal situations, a sliding-window average can be applied to the residual sequence to obtain the actual residual covariance:  
\[\mathbf{a}_{r}=\frac{1}{N}\sum_{i=k-N+1}^{k}\mathbf{r}_{i;k}\mathbf{r}_{i;k}^{T}=\mathbf{H}_{ i;k}\mathbf{P}^{\prime}_{i;k/k-1}\mathbf{H}_{i;k}^{T}+\mathbf{R}_{i;k} \tag{26}\]  
Here, \(\mathbf{P}^{\prime}_{i;k/k-1}\) represents the estimated value of the prediction error covariance matrix within the calculation interval \(N\). The specific value of \(N\) is determined based on experience and the specific situation and is generally chosen to be between 10 and 12. By comparing thecovariance, i.e., when \(\eta>2 or\eta<0.2\), it is considered to be an indication of a fault in the measurement system.  
In this paper, the above two methods are used for fault detection to avoid false alarms or missed detections, thereby reducing the impact of fault signals on the system. If the measurement system fails, the entire system's state information will be affected after the fusion of the main filter information. In order to ensure the normal stability of the system and avoid the isolation of sub-filters affecting the overall system, a simple compensation method was chosen. That is, when a fault is detected, the estimate of the main filter is used instead of the estimate of the faulty sub-filter.  
\[\hat{\mathbf{x}}_{i}=\hat{\mathbf{x}}_{g} \tag{28}\]