calmf1111.m

自己写的一个klman滤波对匀速直线运动物体进行目标跟踪的程序!

T=5;
Q=[0.001 0;0 0.001];
R=[0.25 0;0 5*10^(-4)];
X=[4 8 20 10]';
x=X;
A=[1 0 T 0;0 1 0 T;0 0 1 0;0 0 0 1];
G=[0 0;0 0;1 0;0 1];
P=[1.0 0 0 0;0 1.0 0 0;0 0 0.01 0;0 0 0 0.01];
C=[1 0 0 0;0 1 0 0];
upper=300;
for i=1:T:upper
    W=random('norm',0,0.1,2,1);
    ww=G*W;
    v=mean((ww)*(ww)');
    QQ=diag(v,0);
    V=random('norm',0,0.5,2,1);
    X=A*X+G*W;
    xx1(i)=X(1,1);
    yy1(i)=X(2,1);
    Y=C*X+V;
    xx2(i)=Y(1,1);
    yy2(i)=Y(2,1);
    PP=A*P*A'+QQ;
    H=PP*C'*inv((C*PP*C'+R));
    P=(eye(4)-H*C)*PP;
    x=A*x+H*(Y-C*A*x);
    xx(i)=x(1,1);
    yy(i)=x(2,1);
    delta(i)=(yy(i)-yy1(i))^2+(xx(i)-xx1(i))^2;
end
  t=1:T:upper;
  plot(xx(t),yy(t));
  hold on;
  plot(xx1(t),yy1(t),'g-.+');
  plot(xx2(t),yy2(t),'m');
  grid;
  legend('滤波值','预测值','观测值');
  figure(2);
  plot(t,delta(t));
  grid;