关于雷达系统中目标航迹的检测前跟踪（TBD）matlab代码.rar

%基于粒子滤波的TBD在一个目标的仿真 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(4,N); %1000 particles 第一个目标最初的分布 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(1,N); mm=m; hA=ones(1,N); 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(1,N); n1=sqrt(H)*randn(100,160); n2=sqrt(H)*randn(100,160); z=n1.^2+n2.^2; if k>=5&&k<=20 st=F*st+G*sqrt(Q)*randn(2,1); 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(i,j)=(sqrt(P)*u+n1(i,j))^2+n2(i,j)^2; end end end for i=1:Nr for j=1:Nd h(i,j)=z(2*i,10*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(2,1); elseif m(i)==0 s(:,i)=s0(:,i); end if m(i)==0 for ii=1:Nr for jj=1:Nd u=2*H; hA(i)=hA(i)/u*exp(-h(ii,jj)/u); end end elseif m(i)==1 rk=sqrt(s(1,i)^2+s(2,i)^2); dk=(s(1,i)*s(3,i)+s(2,i)*s(4,i))/rk; bk=atan(s(2,i)/s(1,i)); for ii=1:Nr for jj=1:Nd rr=60+0.2*ii; dd=0.1+0.015625*jj; u=P*exp(-(rr-rk)^2*Lr/R-(dd-dk)^2*Ld/D-(bk*1000)^2*Lb/B)+2*H; hA(i)=hA(i)/u*exp(-h(ii,jj)/u); end end end end qsum=sum(hA); for i=1:N hA(i)=hA(i)/qsum; end ss = s; Neff = 1/sum(hA.^2); if Neff < eff for i = 1 : N p_r = rand; p_qtempsum = 0; for j = 1 : N p_qtempsum = p_qtempsum + hA(j); if p_qtempsum >= p_r ci1=sqrt(0.2*10/N)*randn; ci2=sqrt(0.2*0.2/N)*randn; ci3=sqrt(0.2*0.25/N)*randn; ci4=sqrt(0.2*0.2/N)*randn; c=[ci1 ci2 ci3 ci4]'; m(i)=mm(j); s(:,i) = ss(:,j)+c; break; end end end end no=0;on=0; for i = 1:N if m(i) == 1 on=on+1; elseif m(i) == 0 no = no+1; end end hd1(k) = on/N; hd2(k) = no/N; state = mean(s,2); if on > no lastx(k) = state(1); lasty(k) = state(2); lastxv(k) = state(3); lastyv(k) = state(4); end end %------------------------------------------------- figure; %estimated mode plot(hd1,'b.-'); hold on; plot(hd2,'r*-'); xlabel('Time'); ylabel('Probability'); legend('on','no'); hold off; %-------------------------------------------------