检测前跟踪小程序

%基于粒子滤波的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（4N）;   %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（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;