ts_wtkalman.m

数据融合的问题

function[xigema,kf,kp,pusaif,e,xjian,xxjian]=TS_WTKalman(fai,gama,H,Q,R,y)
%*************************************************************************
%            稳态Kalman滤波算法通式
%*************************************************************************
%本程序考虑线性离散时不变随机系统。系统模型为x(t+1)=fai*x(t)+gama*w(t)
%y(t)=H(t)*x(t)+v(t).有6个参数：状态转移阵fai，输入噪声系数gama，观测阵H，输入
%噪声方差Q，观测噪声方差R，观测y
%**************************************************************************
n=size(fai,1); % the dimension of fai
L=size(R,1);   % the dimension of R
Bushu=size(y,2)% the dimension of y
pp(:,:)=eye(n);
%*****************************************Riccati equation****************
for i=2:Bushu+1;
    temp=pp(:,n*(i-2)+1:n*(i-1));
    pp(:,n*(i-1)+1:n*i)=fai*[temp-temp*H'*inv(H*temp*H'+R)*H*temp]*fai'+...
        gama*Q*gama';
    kf(:,L*(i-1)+1:L*(i-1)+L)=temp*H'*inv(H*temp*H'+R);
    kp(:,L*(i-1)+1:L*(i-1)+L)=fai*kf(:,L*(i-1)+1:L*(i-1)+L);
end
%--------------------------------------------------------------------------
kf=kf(:,end-L+1:end);kp=kp(:,end-L+1:end); %得到稳态值
pusaif=(eye(n)-kf*H)*fai;
pusaip=fai*(eye(n)-kf*H);
xigema=pp(:,end-n+1:end);
% Filtering  Algorithm
xjian(:,Bushu)=zeros(1,n)';
for i=2:Bushu
    xjian(:,i)=pusaif*xjian(:,i-1)+kf*y(:,i);
end
% Predtecting Algorithm
xxjian(:,Bushu)=zeros(1,n)';
for i=2:Bushu
    e(:,i)=y(:,i)-H*xxjian(:,i-1);
    xxjian(:,i)=pusaip*xxjian(:,i-1)+kp*y(:,i);
end