如何改进基于Sage-Husa的自适应卡尔曼滤波？

卡尔曼滤波在信号处理方面用的是比较多的，资料也是非常多的，这里就不写了。自适应卡尔曼滤波也有很多文献有相关的介绍，其中用的比较多的有基于Sage-Husa算法实现的。这段时间刚好用到，顺便做了一个，参考了一些网上的程序如：


https://zhuanlan.zhihu.com/p/100074040


基本卡尔曼以及状态和测量方程方面采用了原来程序的一些内容。Sage-Husa部分进行了改进，主程序如下：


close all;
clear all;
N=1000;
% inlitial Kalman Filter
qu=1;
F = [0.0673 0.1553; 1 0];
G = [1 -0.575 ; 0 0];
H = [1 0];
a=randn(1,N)*sqrt(qu);
Q0=diag([qu qu]); %process noise
R0 = 1;   %measurement noise
P0=eye(2)*10000;
y=zeros(1,N);
y(1)=a(1);
y(2)=a(2)-0.575*a(1);
for k=3:N
    y(k)=0.0673*y(k-1)+0.1553*y(k-2)+a(k)-0.575*a(k-1);
end
y = y+sqrt(R0)*randn(1,N);  %add the measure noise
% because the measurement equal the state variable add a noise in one
% dimension, so initiate it with y(1)
X0=[y(1);0];
b=0.8; % parameter of Sage Husa
[X,P]=Sage_HusaKF(F,G,H,Q0,R0,X0,y,P0,b);

% display the result
t=1:N;
figure
subplot(2,1,1);
plot(t,y,'b',t,X(1,:),'--r');
legend('measure','estimated');
title('Measure value and the estimated value');
subplot(2,1,2);
plot(t,y-X(1,:));
title('error');
其中调用的函数Sage_HusaKF如下：

% modified version of Sage-Husa adeptive KF
function [X,P]=Sage_HusaKF(F,G,H,Q0,R,X0,Z,P,b)
    N=size(Z,2);    %lenght of the measure data in the column
    M=length(X0);
    X=zeros(M,N);
    X(:,1)=X0;
    e=zeros(size(Z));
    q = zeros(M,1);
    Q = Q0;
    for k=2:N
        Ptemp=P;
        X_est=F*X(:,k-1);                  %计算一步预测估计：X(k/k-1)
        P_pre=F*P*F'+G*Q*G';               %一步预测估计的均方误差P(k/k-1)
        e(:,k)=Z(:,k)-H*X_est;           %计算残差epsilon(k)
        K=P_pre*H'*pinv((H*P_pre*H')+R);    %k时刻的增益阵
        X(:,k)=X_est+K*e(:,k);           %k时刻的状态估计X(k)
        P = (eye(M)-K*H)*P_pre;%  均方误差矩阵P(k)
% Sage-Husa adeptive KF,这里做了一些修改
        d = (1-b)/(1-b^(N));
        q = (1-d*b^(k-1))*q +d*b^k*(X(:,k)-F*X(:,k-1));
        Q = (1-d*b^(k-1))*Q +d*b^k*(K*e(:,k)*e(:,k)'*K'+Ptemp-F*P*F');
    end
end
下面是一个仿真结果：


效果基本还可以。


在下面的mathworks网站的“File Exchange”板块也上传一个英文版的，两个版本内容基本一致。


https://www.mathworks.com/matlabcentral/fileexchange/89431-adaptive-kalman-via-a-modified-sage-husa-algorithm


关于q和Q的推导如下图：





需要说明的是基本卡尔曼滤波性能与矩阵P的初值关系很小，改进后滤波性能与P的初值相关性很高，在某些情况下可以较大地改进滤波效果。


另外，该改进方法只对该特定的模型有效，其它的模型，试验过几个，都无效 。