# An Integrated Adaptive Kalman Filter for High-Speed UAVs
## 2 Strong Adaptive Kalman Filter (SAKF)
### Algorithm Designs  
Under the condition that the system model and noise characteristic are already known, KF is linear optimal. The SHAKF breaks the constraint that Q-matrix and R-matrix must be known beforehand, and realizes the on-line estimation of system and measurement noise. This method puts more weights on the residual, and can improve the tracking ability in a high-dynamic environment. However, if practical measurement noise is lower than theoretical one or the priori estimation error is too large, R-matrix may lose its positive definiteness, so that the filtering results will diverge. By using fading factor when calculating P-matrix, STF reduces the weights of the prior knowledge and historical information in current state estimation. Therefore, this method is a good way to track sudden changes. The basic assumption of STF, like KF, is that the noise characteristic obeys or approximately obeys Gaussian distribution. However, the noise under the high-dynamic flight condition of UAV cannot be described as Gaussian distribution. For example, there may be hopping points in GNSS data, thus a large estimation error will occur by then. The H\({}_{\infty}\) filter can guarantee the system's availability under the condition of strong interference, but its average accuracy decreases. The proposed method of integrated filtering, strong adaptive Kalman filter, combines the three methods mentioned above. Based on the framework of KF, SAKF adds adaptive estimation step for error covariance matrices, and R-matrix is considered as the criterion whether the filtering process is abnormal or not. In abnormal condition, the algorithm will choose H\({}_{\infty}\) filter as the filtering main body. Otherwise, STF will be chosen. Figure 1 shows the process of SAKF. The whole process includes 4 parts. **Part 1. ** State prediction update equation is given as follows, \(\hat{\textbf{X}}_{k}\) is the priori state estimation at step \(k\). \(\hat{\textbf{X}}_{k/k-1}\) is the estimation from time step \(k-1\) to \(k\). \(\textbf{\Phi}_{k/k-1}\) relates the state at time step \(k\) to the state at step \(k+1\). \[\hat{\textbf{X}}_{k/k-1}=\textbf{\Phi}_{k/k-1}\hat{\textbf{X}}_{k-1} \tag{4}\]  
**Part 2. ** Residual update equation is given as follows, \(\varepsilon_{k}\) is the residual at step \(k\), \(\textbf{Z}_{k}\) is the measurement value at step \(k\), and \(\textbf{H}_{k}\) represent the measurement matrix. \[\varepsilon_{k}=\textbf{Z}_{k}-\textbf{H}_{k}\hat{\textbf{H}}_{k/k-1} \tag{5}\]  
**Part 3. ** Measurement of anomaly detection step  
The adaptive estimation method for the Q-matrix and R-matrix does not conflict with the framework of STF and H\({}_{\infty}\) filter, but the intermediate steps of STF and H\({}_{\infty}\) filter are conflicts with each other. So it is necessary to judge whether the measurement information is abnormal. The algorithm can use STF under normal condition, or switch to the H\({}_{\infty}\) filter under abnormal conditions. Firstly, we should set the upper bound of R-matrix, and the matrix of maximum values named \(\textbf{R}_{\max}\) is given as follows,  
\[\textbf{R}_{\max}=diag(R_{\max}^{1},R_{\max}^{2},\cdots,R_{\max}^{m}) \tag{6}\]  
Figure 1: The process of the strong adaptive Kalman filter. The whole flow chart includes state prediction, residual, and measurement anomaly detection, filtering of the main body, and Q Adaption. where \(m\) is the number of measurement vectors. Before each update, a detection should be made to check whether the estimated value of the \(i^{th}\) vector in R-matrix is smaller than \(R_{\text{max}}^{i}\) (\(i=1,2,3\ldots,m\)). The inequality is given as follows, whose result is considered as the criterion of anomaly or divergence. Here, the superscript \(i\) means this variable corresponding the \(i^{th}\) vector in R-matrix.