雷达目标跟踪MATLAB源程序（卡尔曼滤波算法应用）

下面是利用卡尔曼滤波的方法，实现雷达对目标的跟踪。
参数：
一目标沿水平方向运动，起始位置为（－2000米，1000米），运动速度为15米/秒，扫描周期T＝2秒，采用蒙特卡洛方法对跟踪滤波器进行仿真，仿真次数为100次。

下面为源代码，已经仿真，没有问题，现在正在学习卡尔曼滤波的同学可以参考学习。请大牛们多多指教！


源程序：

clc;

clear;

T=2;%雷达扫描周期

num=100;%滤波次数

%*******************************产生真实轨迹**********************************

N=800/T;

x=zeros(N,1);y=zeros(N,1);

vx=zeros(N,1);vy=zeros(N,1);

x(1)=-2000;

y(1)=1000;

vx=15;vy=0;

ax=0;ay=0;

var=100;

%产生真实轨迹

for i=1:N-1

 x(i+1)=x(i)+vx*T+0.5*ax*T^2;

 y(i+1)=y(i)+vy*T+0.5*ay*T^2;

end

nx=zeros(N,1);ny=zeros(N,1);

nx=100*randn(N,1);

ny=100*randn(N,1);

zx=x+nx;zy=y+ny;

%滤波50次

for m=1:num

  z=2:1;

 xks(1)=zx(1);

 yks(1)=zy(1);

 xks(2)=zx(2);

  yks(2)=zy(2);

 o=4:4;g=4:2;h=2:4;q=2:2;xk=4:1;perr=4:4;

 o=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];

  h=[1 0 0 0;00 1 0];

 g=[T/2,0;T/2,0;0,T/2;0,T/2];

  q=[10000 0;010000];

  perr=[var^2var^2/T 0 0

 var*var/T 2*var^2/(T^2) 0 0

  0 0var^2 var^2/T

  0 0var^2/T 2*var^2/(T^2)];

 vx=(zx(2)-zx(1))/2;

 vy=(zy(2)-zy(1))/2;

 xk=[zx(1);vx;zy(1);vy];

  %卡尔曼滤波开始

  forr=3:N;

 z=[zx(r);zy(r)];

 xk1=o*xk;

 perr1=o*perr*o';

 k=perr1*h'*inv(h*perr1*h'+q);

 xk=xk1+k*(z-h*xk1);

 perr=(eye(4)-k*h)*perr1;

 xks(r)=xk(1,1);

  yks(r)=xk(3,1);

 vkxs(r)=xk(2,1);

 vkys(r)=xk(4,1);

 xk1s(r)=xk1(1,1);

 yk1s(r)=xk1(3,1);

 perr11(r)=perr(1,1);

 perr22(r)=perr(2,2);

 rex(m,r)=xks(r);

 rey(m,r)=yks(r);

  end %结束一次滤波

  end

ex=0;ey=0;

eqx=0;eqy=0;

ey1=0;

ex1=N:1;ey1=N:1;

%计算滤波的均值

%计算滤波误差的均值

for i=1:N

  for j=1:num

 ex=ex+x(i)-rex(j,i);

 ey=ey+y(i)-rey(j,i);

  end

 ex1(i)=ex/num;

 ey1(i)=ey/num;

 ex=0;eqx=0;ey=0;eqy-0;

end

%绘图

figure(1);

plot(x,y,'k-',zx,zy,'b:',xks,yks,'r-.');

legend('真实轨迹','观测样本','估计轨迹')'

figure(2);

plot(ey1);

legend('x方向平均误差');