sh_max.m

对运动声目标进行航迹跟踪

function X = sh_max(xuw,xud,Xs_fft,id_v,delta,xuvar)
 

d_a=0.04;
vs=345;
L=6;
fs=48e3;
num1=1024;
f_s=fs/num1;
id_l=length(id_v);
samnum=size(xuw,2);
Lv=[];
for lp1=1:samnum
    
Rthite = xuvar(1);%% the variance of angel-of-arrival;
RQv = xuvar(2);%% the variance of Q;
Rfai = xuvar(3);%% the variance of heading direction;
      
          Yw=[];
          Aw=[]; 
   sita=xuw(1,lp1);    
   for lp2 = 1:id_l
       
         Xw=Xs_fft(id_v(lp2),:).';
         Yw=[Yw;Xw];
         
         fk=id_v(lp2)*f_s;
         af=exp(-j*2*pi*fk*d_a*[0:L-1].'*sin(sita*pi/180)./vs);
         Bf=[];
         if lp2==1
             Bf = [af;zeros((id_l-1)*L,1)];
         else 
             Bf_l=zeros((lp2-1)*L,1);
             Bf_r=zeros((id_l-lp2)*L,1);
             Bf = [Bf_l;af;Bf_r];
         end
         
%          Af((lp2-1)*L+1:(lp2-1)*L+L,:) = af;
         
         Aw=[Aw Bf];
      
     end
     
     mtx=(1/L).*eye(id_l);
     knerl=eye(id_l*L)-(Aw*mtx*Aw');
     Ly=exp(1/delta^2).*Yw'*knerl*Yw;
     
  
     fct1=(1/((2*pi)^2*Rthite*RQv*Rfai));
     fct2=-(1/(2*Rthite^2))*(xuw(1,lp1)-xud(1,lp1))^2;
     fct3=-(1/(2*RQv^2))*(xuw(2,lp1)-xud(2,lp1))^2;
     fct4=-(1/(2*Rfai^2))*(xuw(3,lp1)-xud(3,lp1))^2;
     Lx=fct1*exp(fct2+fct3+fct4);
     
     Lxy=Lx*Ly;
     Lv=[Lv;Lxy];
     
 end
 
 [max_iv,max_id]=max(Lv);
 
 X=xuw(:,max_id);