# Sage-Husa Algorithm Based on Adaptive Double Forgetting Factors
## 2 Adaptive Sage-Husa Filtering Algorithm
### Improvement of Sage-Husa Algorithm
The measurement noise covariance is updated using the forgetting factor related to the priori residual; the process noise covariance is updated using the forgetting factor related to the posteriori residual. The \(L\) has the following two forms of Equations (20) and (21):  
\[L_{1}=-\frac{\sum\limits_{i=k-S}^{k-1}e_{i}e_{i}^{T}}{S} \tag{20}\]  
\[L_{2}=-\frac{\sum\limits_{i=k-S}^{k-1}\delta_{i}\delta_{i}^{T}}{S}, \tag{21}\]  
where \(S\) represents the window width, and \(k\) represents the current moment. It can be seen from the above formulas that the variable is related to the window width and the error. The value of the forgetting factor depends on the priori residual and the posteriori residual under the premise that the window width is unchanged. The above methods to obtain two adaptive forgetting factors change the fixed forgetting factor in the traditional Sage-Husa algorithm so that the adaptive forgetting factors can follow the state to change and have better adaptability for the updating of the noise statistical characteristics. Thus, \(R_{k}\) and \(Q_{k}\) are modified as follows:  
\[R_{k}=(1-d_{k})R_{k-1}+d_{k}\Big{(}e_{k}e_{k}{}^{T}\Big{)} \tag{13}\]0\[R_{k}=(1-d_{k})R_{k-1}+d_{k}\Big{(}e_{k}e_{k}{}^{T}\Big{)} \tag{13}\]1  
where \(d_{1,k}=(1-b_{1})/(1-b_{1}{}^{k})\), \(b_{1}=\lambda_{\text{min}}+(1-\lambda_{\text{min}})*2^{L_{1}}\), \(d_{2,k}=(1-b_{2})/(1-b_{2}{}^{k})\), and \(b_{2}=\lambda_{\text{min}}+(1-\lambda_{\text{min}})*2^{L_{2}}\). (3) Re-update \(R_{k}\) by the covariance matching technology  
As the number of iterations increases, the cumulative error will increase to a certain extent, and eventually, the filter using Sage-Husa will probably diverge. In order to avoid this kind of situation, covariance matching technology is proposed as the criterion of filtering divergence. That is, in the filtering process, the actual innovation is constantly tested so as to judge whether the actual innovation is consistent with the theoretical value. The covariance matching technique uses the priori residual as the judgment basis. In this paper, this method is used as the judgment basis for whether the measurement noise is needed to re-update. The following formula is the basis for this decision:  
\[R_{k}=(1-d_{k})R_{k-1}+d_{k}\Big{(}e_{k}e_{k}{}^{T}\Big{)} \tag{13}\]2  
If Equation (24) does not hold true, the measurement noise needs to be updated again. Add a coefficient \(\alpha_{k}=\frac{e_{k}^{T}e_{k}}{tr(H_{k}P_{k|k-1}H_{k}^{T}+R_{k})}\) to Equation (18) as follows:  
\[R_{k}=(1-d_{k})R_{k-1}+d_{k}\Big{(}e_{k}e_{k}{}^{T}\Big{)} \tag{13}\]3  
Above all, the proposed method in this paper can be used to update the statistical characteristics of measurement noise and process noise in real time and guarantee the covariance matrix of measurement noise is positive definite, and the covariance matrix of process noise is non-negative definite, which improves the accuracy of the extended Kalman filter. (4) Basic flow of the algorithm  
The calculation steps of the improved Sage-Husa algorithm in a sampling period are as follows:  
1. Calculate the state prediction \(\hat{X}_{k|k-1}=f\big{(}\hat{X}_{k-1},U_{k-1}\big{)}\), the state prediction error covariance matrix \(P_{k|k-1}=\Phi_{k-1}P_{k-1|k-1}\Phi_{k-1}{}^{T}+Q_{k-1}\), and the prior residual \(e_{k}=Z_{k}-h\Big{(}\hat{X}_{k|k-1}\Big{)}\). 2. Obtain the adaptive forgetting factor using the prior residual as follows: \(b_{1}=\lambda_{\text{min}}+(1-\lambda_{\text{min}})*2^{L_{1}}\), \(L_{1}=-\frac{\sum\limits_{k=-1}^{k-1}e_{k}t^{T}}{S}\). 3. Calculate the weighted coefficient according to the forgetting factor as follows: \(d_{1,k}=\frac{(1-b_{1})}{(1-b_{1}^{T})}\). 4. Update the measurement noise as follows: \(R_{k}=(1-d_{1,k})R_{k-1}+d_{1,k}(\delta_{k-1}\delta_{k-1}^{T}+H_{k-1}P_{k-1|k -1}H_{k-1}^{T})\). 5.