# An Integrated Adaptive Kalman Filter for High-Speed UAVs
## 2 Strong Adaptive Kalman Filter (SAKF)
### Algorithm Designs
\[R_{k/k-1}^{i}=\left(\varepsilon_{k}^{i}\right)^{2}-\mathbf{H}_{k}^{i}\mathbf{P}_{k/k- 1}^{i}\mathbf{(H}_{k}^{i})^{T}<R_{\text{max}}^{i} \tag{7}\]  
When every \(R_{k/k-1}^{i}\) satisfies the condition of inequality (7), we believe the measurement results are usable and the filter's work state is stable. So the R-matrix update equation can be showed as follows,  
\[\hat{\mathbf{R}}_{k}=(1-\beta_{k})\hat{\mathbf{R}}_{k-1}+\beta_{k}(\varepsilon_{k} \varepsilon_{k}^{T}-\mathbf{H}_{k}\mathbf{P}_{k/k-1}\mathbf{H}_{k}^{T}) \tag{8}\]  
Then update the measurement information using the updated equation of STF, which is given as follows:  
\[\left\{ \begin{array}{l}\mathbf{P}_{k/k-1}=L_{k}\mathbf{\Phi}_{k/k-1}\mathbf{P}_{k-1}\mathbf{ \Phi}_{k/k-1}^{T}+\hat{\mathbf{Q}}_{k-1}\\ \mathbf{K}_{k}=\mathbf{P}_{k/k-1}\mathbf{H}_{k}^{T}(\mathbf{H}_{k}\mathbf{P}_{k/k-1}\mathbf{H}_{k}^{T} +\mathbf{R}_{k})^{-1}\\ \hat{\mathbf{X}}_{k}=\hat{\mathbf{X}}_{k/k-1}+\mathbf{K}_{k}(\mathbf{Z}_{k}-\mathbf{H}_{k}\hat{ \mathbf{X}}_{k/k-1})\\ \mathbf{P}_{k}=(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k/k-1}(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_ {k})^{T}+\mathbf{K}_{k}\mathbf{R}_{k}\mathbf{K}_{k}^{T}\end{array} \right. \tag{9}\]  
Here \(L_{k}\) represents the diagonal matrix of fading factors, which is given as follows,  
\[L_{k}=diag(\lambda_{1k},\lambda_{2k},\cdots\lambda_{nk}) \tag{10}\]  
Each fading factor can be gotten by Equation (11). \[\lambda_{ik}=\max\{1,\alpha_{i}C_{k}\} \tag{11}\]  
where \(\alpha_{i}\) is a fixed parameter, and the value of \(C_{k}\) can be obtained by Equations (12)-(16). \[\alpha=diag(\alpha_{1},\alpha_{2},\cdots\alpha_{k}) \tag{12}\]  
\[\mathbf{V}_{k}=\left\{ \begin{array}{ll}\varepsilon_{0}\varepsilon_{0}^{T},&k=0 \\ \frac{\rho}{1+\rho}\mathbf{V}_{k-1}+\frac{1}{1+\rho}\varepsilon_{k}\varepsilon_{k}^ {T},&k\geq 1\end{array} \right. \tag{13}\]  
\[\mathbf{M}_{k}=\alpha\mathbf{\Phi}_{k/k-1}\mathbf{P}_{k-1}\mathbf{\Phi}_{k/k-1}^{T}\mathbf{H}_{k}^ {T}\mathbf{H}_{k} \tag{14}\]  
\[\mathbf{N}_{k}=\mathbf{V}_{k}-\mathbf{R}_{k}-\mathbf{H}_{k}\mathbf{Q}_{k-1}\mathbf{H}_{k}^{T} \tag{15}\]  
\[\mathbf{C}_{k}=\frac{Tr[\mathbf{N}_{k}]}{Tr[\mathbf{M}_{k}]} \tag{16}\]  
where the initial value of \(\mathbf{V}_{k}\) is set as the covariance matrix of residual. Then the forgetting factor \(\rho\) adjusts the ratio between \(\mathbf{V}_{k-1}\) and \(\varepsilon_{0}\varepsilon_{0}^{T}\) in the following iterations. As \(0<\rho\leq 1\), \(\frac{\rho}{1+\rho}\in(0,0.5]\). This indicates that a lager weight is put on the current information than historical information. Therefore, the residual plays an important role in the filtering process, and the tracking ability can be improved in this way. If there exists \(R_{k/k-1}^{i}\) which does not satisfy \(R_{k/k-1}^{i}\leq R_{\text{max}}^{(i)}\), then the elements of R-matrix are updated as follows,  
\[\hat{R}_{k}^{(i)}=\left\{ \begin{array}{ll}R_{\text{max}}^{(i)}&R_{k/k-1}^{i }\geq R_{\text{max}}^{(i)}\\ (1-\beta_{k})\hat{R}_{k-1}^{(i)}+\beta_{k}R_{k/k-1}^{i}&R_{k/k-1}^{i}<R_{\text {max}}^{(i)}\end{array} \right. \tag{17}\]Then the updated equation of H\({}_{\infty}\) filter is used as the filtering step:  
\[\left\{ \begin{array}{l}\mathbf{K}_{k}=\mathbf{P}_{k-1}(\mathbf{I}-\theta\mathbf{P}_{k-1}+\mathbf{H} _{k}^{T}\mathbf{R}_{k}^{-1}\mathbf{H}_{k}\mathbf{P}_{k-1})^{-1}\mathbf{H}_{k}^{T}\mathbf{R}_{k}^{-1} \\ \mathbf{\hat{X}}_{k}=\mathbf{\hat{X}}_{k/k-1}+\mathbf{\Phi}_{k/k-1}\mathbf{K}_{k}(\mathbf{Z}_{k}- \mathbf{H}_{k}\mathbf{\hat{X}}_{k/k-1})\\ \mathbf{P}_{k}=\mathbf{\Phi}_{k/k-1}\mathbf{P}_{k-1}(\mathbf{I}-\theta\mathbf{P}_{k-1}+\mathbf{H}_{k}^{ T}\mathbf{R}_{k}^{-1}\mathbf{H}_{k}\mathbf{P}_{k-1})^{-1}\mathbf{\Phi}_{k/k-1}^{T}+\mathbf{Q}_{k-1} \end{array} \right. \tag{18}\]  
Set constrain condition for iteration, like Equation (19),  
\[\mathbf{P}_{k}^{-1}-\theta\mathbf{\widetilde{S}}_{k}+\mathbf{H}_{k}^{T}\mathbf{R}_{k}^{-1}\mathbf{ H}_{k}>0 \tag{19}\]  
As \(\theta\) is a key parameter for filtering performance, it is difficult to select an appropriate value.