init_oneparticle.m

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

function Init_oneParticle(k)
%get the range that every soar gets for the particle a; 
load p
sonar=[-1.57,-0.87222,-0.52333,-0.17444,0.17444,0.52333,0.87222,1.57];
pi=3.1415926;
i=0;j=0;
d=0;
for i=1:1:8
    %----------------------------------------------------------------
    y1=0;
    x1=p(k,1)-p(k,2)./(tan((p(k,3)./180)*pi+sonar(i)));
    if(x1>=0&&x1<6.7&&angle_ok(p(k,1),p(k,2),x1,y1,(p(k,3)+sonar(i))))
         d(1)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);
    else
        d(1)=6;       
    end
    %-----------------------------------------------------------------
        x1=6.7;
		y1=tan((p(k,3)/180)*pi+sonar(i))*(x1-p(k,1))+p(k,2);
		if(y1>=0&&y1<1.5&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi)) 		
			d(2)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(2)=6;
        end
    %------------------------------------------------------------------        
        y1=1.5;
		x1=p(k,1)+(y1-p(k,2))/tan((p(k,3)/180)*pi+sonar(i));
		if(x1>6.7&&x1<=7.4&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi)) 		
			d(3)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(3)=6;
        end
		%------------------------------------------------------------
		x1=7.4;          
        y1=tan((p(k,3)/180)*pi+sonar(i))*(x1-p(k,1))+p(k,2);
		if(y1>1.5&&y1<3.2&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi))  		
			d(4)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(4)=6;
        end
        %------------------------------------------------------------
        y1=3.2;
		x1=p(k,1)+(y1-p(k,2))/tan((p(k,3)/180)*pi+sonar(i));
		if(x1>6.5&&x1<=7.4&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi))		
			d(5)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(5)=6;
        end
        %------------------------------------------------------------
		x1=6.5;          
        y1=tan((p(k,3)/180)*pi+sonar(i))*(x1-p(k,1))+p(k,2);
		if(y1>3.2&&y1<4.1&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi)) 		
			d(6)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(6)=6;
        end
		%-------------------------------------------------------------
         y1=4.1;
		x1=p(k,1)+(y1-p(k,2))/tan((p(k,3)/180)*pi+sonar(i));
		if(x1>3.3&&x1<=6.5&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi)) 		
			d(7)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(7)=6;
        end
        %-------------------------------------------------------------
        x1=3.3;          
        y1=tan((p(k,3)/180)*pi+sonar(i))*(x1-p(k,1))+p(k,2);
		if(y1>4.1&&y1<=4.7&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi))  
		d(8)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);
		else 
            d(8)=6;
        end
		%--------------------------------------------------------------
        y1=4.7;
		x1=p(k,1)+(y1-p(k,2))/tan((p(k,3)/180)*pi+sonar(i));
		if(x1>0.7&&x1<=3.3&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi)) 
					d(9)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);
		else 
            d(9)=6;
        end
		%--------------------------------------------------------------
        x1=0.7;          
        y1=tan((p(k,3)/180)*pi+sonar(i))*(x1-p(k,1))+p(k,2);
		if(y1>3.4&&y1<=4.7&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi))  		
			d(10)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(10)=6;
        end
		%------------------------------------------------------------
        y1=3.4;
		x1=p(k,1)+(y1-p(k,2))/tan((p(k,3)/180)*pi+sonar(i));
		if(x1>=0&&x1<=0.7&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi))		
			d(11)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(11)=6;
        end
		%-------------------------------------------------------------
		x1=0.0;          
        y1=tan((p(k,3)/180)*pi+sonar(i))*(x1-p(k,1))+p(k,2);
		if(y1>=0&&y1<=3.4&&angle_ok(p(k,1),p(k,2),x1,y1,p(k,3)+sonar(i)*180/pi)) 		
			d(12)=Distance_pointTopoint(p(k,1),p(k,2),x1,y1);            
		else 
            d(12)=6;
        end
     %-------------------------------------------------------------- 	 
		for j=1:1:12
			if(d(j)<d(1)) 
                d(1)=d(j);
            end                
        end	
        %-----------------------------------------------------------
		p(k,i+4)=d(1)+randn(1)/10;        
        
end
save('p','p')