sage_husakf.m

一种改进的自适应滤波器的状态估计

function [X,e,P]=Sage_HusaKF(F,G,H,Q0,R0,X0,Z,P0,b,s);
% Sage-Husa adeptive KF

N=length(Z);
M=length(X0);
X=zeros(M,N);
X(:,1)=X0;
s=1*eye(2);
P=P0;
q0 = zeros(M,1);
r0 = 0;
q = q0;
r = r0;
Q = Q0;
R = R0;
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*(eye(M)-K*H)'+K*R*K';  %均方误差矩阵P(k)
    
% %     sage-Husa更新Q，R，q,r
%     d = (1-b)/(1-b^(k+1));
%     r = (1-d)*r +d*(Z(:,k)-H*X_est);
%     q = (1-d)*q +d*(X(:,k)-F*X(:,k-1));
%     R = (1-d)*R +d*(Z(:,k)*Z(:,k)'-H*P*H');
%     Q = (1-d)*Q +d*(K*e(:,k)*e(:,k)'*K'+P-F*P*F');
%    
      r = 1/k*((k-1)*r +Z(:,k)-H*X_est);
    
      q = 1/k*((k-1)*q+X(:,k)-F*X(:,k-1));
    
      R = 1/k*((k-1)*R+Z(:,k)*Z(:,k)'-H*P*H');
     
      Q = 1/k*((k-1)*Q+K*e(:,k)*e(:,k)'*K'+P-F*P*F');
%     
end