function?[XP]=Sage_HusaKF（FGHQRX0ZP）
%?Sage-Husa?adeptive?KF?
?
N=50;?
M=100;?
X=zeros（MN）;?
%?X（:1）=X0;?
%?s=1*eye（2）;?
q=?zeros（M1）;?
r0?=?0;??
b?=?0.97;
for?k=2:N?
????X_est=F*X（:k-1）+q;??????????????????%計算一步預測估計：X（k/k-1）?
????P_pre=F*P*F‘+G*Q*G‘;???????????????%一步預測估計的均方誤差P（k/k-1）?
????e（:k）=Z（:k）-H*X_est-r;???????????%計算殘差epsilon（k）?
????K=P_pre*H‘*inv（（H*P_pre*H‘）+R）;????%k時刻的增益陣?
????X（:k）=X_est+K*e（:k）;???????????%k時刻的狀態估計X（k）?
????P?=?（eye（M）-K*H）*P_pre*（ey