resample_paricles.asv

基于声纳信息的粒子滤波算法

  function ReSample_paricles
  %------------------------------------------------------------------------
  % Resample particles to get more crowed particles .this is ok;
  % 2007.09.29
  %------------------------------------------------------------------------
  global num
  global  p 
  %------------------------------------------------------------------------
  %------------------------------------------------------------------------
  for i=1:1:num-1
    tmp(1,1:12)=p(i,1:12);
    k=i;
    tt=p(i,4);
      for j=i+1:1:num        
          if(p(j,4)>=tt)
            tmp(1,1:12)=p(j,1:12);
            tt=p(j,4);
            k=j;
         end
     end
     p(k,1:12)=p(i,1:12);
     p(i,1:12)=tmp(1,1:12);   
  end  
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
  for i=1:1:num	  	
	   value_one(i,4)=p(i,4);         
  end
      value_add(1)=value_one(1,4);
     for i=2:1:num           
         value_add(i)=value_add(i-1)+value_one(i,4);        
     end
     for i=1:1:num           
	
%      l:    
     all_pvalue=rand(1);
        if(all_pvalue<value_add(1))           
		    value_one(i,1:12)=p(1,1:12);           
        end
		for j=2:1:num		
		   if((all_pvalue<value_add(j))&&(all_pvalue>value_add(j-1)))
                if(p(j,4)>1./(2*num))
                value_one(i,1:12)=p(j,1:12); 
                else
                   value_one(i,1:12)=p(1,1:12);
                end
                break; 
           end
        end
    end
    for i=1:1:num	
		p(i,1:12)=value_one(i,1:12);
    end
 %---------------------------------------------------------------------------