kalman.m

kalman滤波程序

 function KalmanFilter()  
  fp1=fopen('kal.dat','w');
 
 fid=fopen('result1.dat','r');
 c=fread(fid);
 b=char(c');
 ZZZ=str2num(b);
 fclose(fid); 

 

A=[0.9 0.1;-0.1 0.8];
B=[1 0]';
C=[0 1];

wk=idinput(100,'RGS',[0 1],[-1,1]);
vk=idinput(100,'RGS',[0 1],[-1,1]);



 P0=[99 0;0 99];
 Qk=var(wk);
 Rk=var(vk);
 X(:,1)=[3 -3]';
 
 
  for k=2:100
     

  X1=(A*X(:,(k-1))); %(2,1)
  
  P1=(A*P0*A'+B*Qk*B');%一步预测误差的方差阵(2,2)
  
  K=(P1*C'*inv(C*P1*C'+Rk));%滤波增益阵(2,1)

  ZZ=ZZZ(k,2)+vk(k);

  X(:,k)=X1+K*(ZZ-C*X1)

  P=((eye(2)-K*C)*P1*(eye(2)-K*C)'+K*Rk*K');%滤波误差的方差阵
  
  P0=P;
  end

for k=1:100
 
fprintf(fp1,'%8.7f,  %8.7f \n',X(1,k),X(2,k));
 
end

fclose(fp1);