sage-husa滤波

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