# Sage-Husa Algorithm Based on Adaptive Double Forgetting Factors
## 2 Adaptive Sage-Husa Filtering Algorithm
### Improvement of Sage-Husa Algorithm  
(1) Update the statistical characteristics of process noise and measurement noise  
During the process of filtering, \(Q_{k}\) must be non-negative definite and \(R_{k}\) must be positive definite to suppress filter divergence. However, the existence of \(-\Phi_{k-1}P_{k-1|k-1}\Phi_{k-1}{}^{T}\) and \(-H_{k}P_{k|k-1}H_{k}{}^{T}\) in Equations (10) and (6) will not guarantee the positive definiteness of \(Q_{k}\) and \(R_{k}\). To ensure the convergence of the Sage-Husa algorithm, \(-\Phi_{k-1}P_{k-1|k-1}\Phi_{k-1}{}^{T}\) and \(-H_{k}P_{k|k-1}H_{k}{}^{T}\) can be abandoned, showing as Equations (12) and (13) [42]. In other words, this method gives up some filtering precision to ensure the stability of the filter. \[Q_{k}=(1-d_{k})Q_{k-1}+d_{k}(K_{k}e_{k}e_{k}{}^{T}K_{k}{}^{T}+P_{k|k}) \tag{12}\]  
\[R_{k}=(1-d_{k})R_{k-1}+d_{k}\Big{(}e_{k}e_{k}{}^{T}\Big{)} \tag{13}\]  
This paper uses Equation (12) to update \(Q_{k}\), proposing an improved approach to update \(R_{k}\). The value of the priori residual, also known as the innovation, is related to the prediction value, as shown in Equation (6). The posteriori residual is related to the state estimation value, as shown in Equation (14):  
\[\delta_{k}=Z_{k}-h\Big{(}\hat{X}_{k|k}\Big{)}. \tag{14}\]  
Use the law of error propagation to calculate the covariance of the posterior residual as follows [43]:  
\[E(\delta_{k}\delta_{k}^{T})=R_{k}-H_{k}P_{k|k}H_{k}{}^{T} \tag{15}\]  
\[R_{k}=E(\delta_{k}\delta_{k}^{T})+H_{k}P_{k|k}H_{k}{}^{T}. \tag{16}\]  
In practice, the value of \(\delta_{k}\delta_{k}^{T}\) at the present moment replaces the expectation of \(\delta_{k}\delta_{k}^{T}\). Equation (6) can be written in another form of a recursive estimation formula related to the posteriori residual, which is as follows:  
\[R_{k}=(1-d_{k})R_{k-1}+d_{k}R^{\prime}{}_{k}, \tag{17}\]where \(R^{\prime}_{k}=\delta_{k}\delta_{k}^{T}+H_{k}P_{k|k}H_{k}^{T}\). Compared with Equation (6), it can be seen that the formula does not own the part of subtraction, which ensures the positive definiteness of the measurement noise covariance matrix. Actually, to calculate \(R^{\prime}\), it is necessary to know the Kalman gain matrix, but the Kalman gain relies on the measurement noise covariance matrix, falling into an infinite loop. To solve this problem, it is necessary to go back to utilize the mean value of the previous moments by using the moving window. To simplify the calculation processing, this paper chooses only the k-1 moment value. The formula can be written as follows:  
\[R_{k}=(1-d_{k})R_{k-1}+d_{k}(\delta_{k-1}\delta_{k-1}^{T}+H_{k-1}P_{k-1|k-1}H_ {k-1}^{T}). \tag{18}\]  
(2) Adaptive double forgetting factors  
The forgetting factor plays a key role in the performance of the Sage-Husa algorithm and usually takes a constant between 0.95 and 0.99, but the fixed forgetting factor cannot adapt well to the changes in the system. In order to make the algorithm achieve better effects, the forgetting factor should change adaptively with the changes in the system. In the traditional Sage-Husa algorithm, one forgetting factor is used to update the noise statistical properties, which ignores the difference between the measurement noise covariance and process noise covariance, i.e., one forgetting factor does not adapt well to the updating of the two noise covariances. Therefore, this paper provides a method to update the double forgetting factors, as shown in Equation (19):  
\[b=\lambda_{\text{min}}+(1-\lambda_{\text{min}})*2^{L}, \tag{19}\]  
where \(\lambda_{\text{min}}\) is the minimum value of the forgetting factor, usually taken as 0.95, \(L\) is a variable related to the error. In order to adaptively update the noise statistical characteristics, two different forgetting factors are set to update the measurement noise covariance and the process noise covariance, respectively.