雷达机动目标跟踪源程序

附录

附录A 机动目标跟踪与实现源程序

T=2;alpha=0.8; % 加权衰减因子

window=round(1/(1-alpha)); % 检测机动的有效窗口长度

dt=100; % dt=dt_x=dt_y=100

Th=25; % 机动检测门限

Ta=9.49; % 退出机动的检测门限

N=800/T; %采样次数

M=50; %模拟次数

%真实轨迹数据

t=2:2:400;

xo0=2000+0*t;

yo0=10000-15*t;

t=402:2:600;

xo1=2000+0.075*((t-400).^2)/2;

yo1=10000-15*400-(15*(t-400)-0.075*((t-400).^2)/2);

t=602:2:610 ;

xo2=xo1(100)+15*(t-600);

yo2=yo1(100)+0*t;

t=612:2:660;

xo3=xo2(5)+(15*(t-610)-0.3*((t-610).^2)/2);

yo3=yo2(5)-0.3*((t-610).^2)/2;

t=662:2:800;

xo4=xo3(25)+0*t;

yo4=yo3(25)-15*(t-660);

x=[xo0,xo1,xo2,xo3,xo4];

y=[yo0,yo1,yo2,yo3,yo4];

e_x1=zeros(1,N);

e_x2=zeros(1,N);

e_y1=zeros(1,N);

e_y2=zeros(1,N);

px=zeros(1,N);

qy=zeros(1,N);

u=zeros(1,N);

u_a=zeros(1,N);

for j=1:M

no1=100*randn(1,N); % 随机白噪

no2=100*randn(1,N);

for i=1:N;

zx(i)=x(i)+no1(i); % 观测数据

zy(i)=y(i)+no2(i);

z(:,i)=[zx(i);zy(i)];

end

X_estimate(2,:)=[zx(2),(zx(2)-zx(1))/T,zy(2),(zy(2)-zy(1))/T];

X_est=X_estimate(2,:);

P_estimate=[dt^2,dt^2/T,0,0;dt^2/T,(dt^2)*2/(T^2),0,0;0,0,dt^2,dt^2/T;0,0,dt^2/T,(dt^2)*2/(T ^2)];

x1(1)=zx(1); y1(1)=zy(1); x1(2)=X_estimate(2,1); y1(2)=X_estimate(2,3);

u(1)=0; u(2)=0;

u1=u(2);

pp=0;% ０表示非机动，１表示机动

qq=0;

rr=1;k=3;

while k<=N

z1=z(:,k);

[X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1);

X_estimate(k,:)=X_est;

X_predict(k,:)=X_pre;

P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4), P_estimate(4,4)];

x1(k)=X_estimate(k,1);

y1(k)=X_estimate(k,3);

u(k)=u1;

k=k+1;

else

if pp==0 %进入非机动模型

if rr==window+1 %由机动进入非机动模型，为防止回朔，至少递推window＋1次

u1=0;

else

end

while rr>0

z1=z(:,k);

[X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1);

X_estimate(k,:)=X_est;

X_predict(k,:)=X_pre;

P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4), P_estimate(4,4)];

x1(k)=X_estimate(k,1);

y1(k)=X_estimate(k,3);

u(k)=u1;

rr=rr-1;

end

if u(k)>=Th

pp=1;qq=1; %“pp=1，qq=1”表示由非机动进入机动模型else

end

k=k+1;

else %机动模型

if qq==1 %由非机动进入机动模型，需要进行修正

k=k-window-1;

Xm_est=[X_estimate(k-1,:),0,0];

Xm_pre=[X_predict(k,:),0,0];

Pm_estimate=zeros(6,6);

P=P(k-1,:);

m=0;

else %在机动模型中进行递推

Xm_est=Xm_estimate(k-1,:);

end

z1=z(:,k);

[Xm_est,Pm_estimate,ua1,qq,m]=kalmandynamic(Xm_pre,Xm_est,Pm_estimate,z1,k,P,q q,m);

Xm_estimate(k,:)=Xm_est;

x1(k)=Xm_estimate(k,1);

y1(k)=Xm_estimate(k,3);

ua(k)=ua1;

if ua1

X_est=Xm_estimate(k,1:4);

P_estimate=Pm_estimate(1:4,1:4);

pp=0;

rr=window+1;

else