singermodel.m

这是目标作机动时,卡尔曼滤波模型采用singer模型的matlab 程序.

function singermodel
%首先计算状态转移矩阵
a=0.01;    %需要更改
e=2.718281828;
TS=0.001;
deltat=0.001;
F=[0 1 0
   0 0 1
   0 0 -a]; 
B=[0;0;1];
h=[1 0 0];
I=[1 0 0
   0 1 0
   0 0 1];
fi=I;
%%%%%%%%%%%%%%%%求fi%%%%%%%%%%%%%%%%%%%
for i=1:100
  k=1;
   for j=1:i
   k=k*j;
   end
fi=fi+F^i*0.001^i/k;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q=I;
%%%%%%%%%%%求Q=tao*q*tao'%%%%%%%%%%%%%%%
% q=1;
% Q(1)=q*0.5/a^5*(1+2*a*deltat-2*a^2*deltat^2+2/3*a^3*deltat^3-4*a*deltat*e^(-a*deltat)-e^(-a*deltat*2));
% Q(2)=q*0.5/a^4*(1-2*a*deltat+a^2*deltat^2+2*a*deltat*e^(-a*deltat)-2*e^(-a*deltat)+e^(-a*deltat*2));
% Q(3)=q*0.5/a^3*(1-2*a*deltat*e^(-a*deltat)-e^(-a*deltat*2));
% Q(4)=Q(2);
% Q(5)=q*0.5/a^3*(-3+2*a*deltat+4*e^(-a*deltat)-e^(-a*deltat*2));
% Q(6)=q*0.5/a^2*(1-2*e^(-a*deltat)+e^(-a*deltat*2));
% Q(7)=Q(3);
% Q(8)=Q(6);
% Q(9)=q*0.5/a*(1-e^(-a*deltat*2));
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PHIS=1;
Q(1,1)=PHIS*TS^5/20;
% Q(1,1)=1;
Q(1,2)=PHIS*TS^4/8;
Q(1,3)=PHIS*TS^3/6;
Q(2,1)=Q(1,2);
Q(2,2)=PHIS*TS^3/3;
Q(2,3)=PHIS*TS*TS/2;
Q(3,1)=Q(1,3);
Q(3,2)=Q(2,3);
Q(3,3)=PHIS*TS;
vpa(Q,20)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 kk=[0;0;0];
 p=[0.1 0 0
    0 0.1 0
    0 0 0.1];        
 r=0.5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fidr=fopen('randomdata.dat','r');
[s_indat,count]=fscanf(fidr,'%f',58517);
fidw(1)=fopen('x.dat','w');
fidw(2)=fopen('t.dat','w');

x(1)=0.0;  x(2)=0.0;   x(3)=0.0;
time=1;
%T=1.0;

for m=1:58517
z=s_indat(m);
xk0=fi*x';
xk=xk0+kk*(z-xk0(1))';
x=xk';  
pk=fi*p*fi'+Q;

kk=pk*h'*inv(h*pk*h'+r);
p=(I-kk*h)*pk*(I-kk*h)'+kk*r*kk';
time=time+1;
 
fprintf(fidw(1),'%f  ',m); 
fprintf(fidw(1),'%f  ',x(2)); 
fprintf(fidw(1),'\n');
fprintf(fidw(2),'%f ',x(3));    
fprintf(fidw(2),'\n');
end
fclose(fidr);
fclose(fidw(1));
fclose(fidw(2));