resample_paricles.m

粒子滤波算法在机器人定位中的应用

  function ReSample_paricles
  %------------------------------------------------------------------------
  % Resample particles to get more crowed particles .this is ok;
  % 2007.09.29
  %------------------------------------------------------------------------
  load num
  load p 
  %------------------------------------------------------------------------
     tmp(1,1:12)=0;
    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 
%--------------------------------------------------------------------------
   value_one(1:num,1:12)=0;
  for i=1:1:num/2	  	
	   value_one(i,4)=p(i,4);         
   end
     for i=1:1:num/2       
	   all_pvalue=0;		 
		 for j=1:1:i		 
			 all_pvalue=all_pvalue+value_one(i,4);
         end
         value_add(i)=all_pvalue;
     end
     for i=1:1:num           
		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/2	
		   if((all_pvalue<=value_add(j))&&(all_pvalue>value_add(j-1)))
             value_one(i,1:12)=p(j,1:12);            
             break;
             end
        end
    end
    for i=1:1:num
		p(i,1:12)=value_one(i,1:12);
    end    
	%for k=num:1:num	    
     %  p(k,1)=7.4*rand(1);
      % p(k,2)=4.7*rand(1);
       %p(k,3)=360*rand(1);
       %p(k,4)=1/num;
 %---------------------------------------------------------------------------
     %Init_oneParticle(k);
     %end
   save('p','p');