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;??????????????????%計(jì)算一步預(yù)測(cè)估計(jì)：X（k/k-1）?
????P_pre=F*P*F‘+G*Q*G‘;???????????????%一步預(yù)測(cè)估計(jì)的均方誤差P（k/k-1）?
????e（:k）=Z（:k）-H*X_est-r;???????????%計(jì)算殘差epsilon（k）?
????K=P_pre*H‘*inv（（H*P_pre*H‘）+R）;????%k時(shí)刻的增益陣?
????X（:k）=X_est+K*e（:k）;???????????%k時(shí)刻的狀態(tài)估計(jì)X（k）?
????P?=?（eye（M）-K*H）*P_pre*（ey