文档介绍：源程序::
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+*ax*T^2;
y(i+1)=y(i)+vy*T+*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;0 0 1 0];
g=[T/2,0;T/2,0;0,T/2;0,T/2];
q=[10000 0;0 10000];
perr=[var^2 var^2/T 0 0
var*var/T 2*var^2/(T^2) 0 0
0 0 var^2 var^2/T
0 0 var^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];
%卡尔曼滤波开始
for r=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);