%基于粒子濾波的TBD在一個(gè)目標(biāo)的仿真
clear;
N?=?2000;??
eff?=?800;
T?=?1;?????
Nr?=?50;Nd?=?16;Nb?=?1;???
R?=?50;D?=?16;B?=?1;??
Lr?=?10000;Ld?=?30000;Lb?=?1;
P?=?12;
tf?=?30;
amaxx?=?0.005;amaxy?=?0.005;
F?=?[?1?0?T?0
??????0?1?0?T
??????0?0?1?0
??????0?0?0?1];
G?=?[1/3*amaxx*T^2?0
?????0?1/3*amaxy*T^2
?????1/2*amaxx*T?0
?????0?1/2*amaxy*T];?
Q?=?1;??
H?=?0.3;?

%?初始化
s?=?rand（4N）;???%1000?particles?第一個(gè)目標(biāo)最初的分布
s（1:）=60+6*s（1:）;
s（2:）=-0.1+0.2*s（2:）;
s（3:）=0.4*s（3:）;
s（4:）=-0.1+0.2*s（4:）;
s0?=?s;

m=zeros（1N）;
mm=m;
hA=ones（1N）;

intervelr=（70-60）/Nr/2;
interveld=（0.35-0.1）/Nd/10;
r=60:intervelr:70;??
d=0.10:interveld:0.35;


st=[63;0;0.2;0];



for?k?=?1?:?tf
????????????
????hA=ones（1N）;
????
???
?????n1=sqrt（H）*randn（100160）;
?????n2=sqrt（H）*randn（100160）;
?????z=n1.^2+n2.^2;
????
????if?k>=5&&k<=20
????????st=F*st+G*sqrt（Q）*randn（21）;
????????x=st（1）;
????????v=st（3）;
????????st（2）=0;
????????st（4）=0;????????
????????truex（k）=st（1）;
????????truey（k）=st（2）;
????????for?i=1:100
????????????for?j=1:160
????????????????u=exp（-Lr*（r（i+1）-x）^2/2/R-Ld*（d（j+1）-v）^2/2/D）;
????????????????z（ij）=（sqrt（P）*u+n1（ij））^2+n2（ij）^2;
????????????end
????????end????????
????end
????
????for?i=1:Nr
????????for?j=1:Nd
????????????h（ij）=z（2*i10*j）;
????????end
????end
????
??
????for?i=1:N
????????if?m（i）==0
????????????if?rand<0.2?
????????????????mm（i）=1;
????????????else
????????????????mm（i）=0;
????????????end
????????elseif?m（i）==1
????????????if?rand>0.8?
????????????????mm（i）=0;
????????????else
????????????????mm（i）=1;
????????????end
????????end
????end

????m?=?mm;
????
???
????for?i=1:N
????????
????????if?m（i）==1
????????????s（:i）=F*s（:i）+G*sqrt（Q）*randn（21）;?
????????elseif?m（i）==0
????????????s（:i）=s0（:i）;
????????end
????????
????????
????????
????????if?m（i）==0???
????????????for?ii=1:Nr
????????????????for?jj=1:Nd
????????????????????u=2*H;
???