init_robottestsonar.m

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

function Init_RobottestSonar
load Robottest
%------------------------------------------------------------------------
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=Robottest(1)-Robottest(2)./(tan((Robottest(3)./180)*pi+sonar(i)));
    if(x1>=0&&x1<6.7&&angle_ok(Robottest(1),Robottest(2),x1,y1,(Robottest(3)+sonar(i))))
         d(1)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);
    else
        d(1)=6;       
    end
 %-----------------------------------------------------------------
        x1=6.7;
		y1=tan((Robottest(3)/180)*pi+sonar(i))*(x1-Robottest(1))+Robottest(2);
		if(y1>=0&&y1<1.5&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi)) 		
			d(2)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(2)=6;
        end
    %------------------------------------------------------------------        
        y1=1.5;
		x1=Robottest(1)+(y1-Robottest(2))/tan((Robottest(3)/180)*pi+sonar(i));
		if(x1>6.7&&x1<=7.4&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi)) 		
			d(3)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(3)=6;
        end
		%------------------------------------------------------------
		x1=7.4;          
        y1=tan((Robottest(3)/180)*pi+sonar(i))*(x1-Robottest(1))+Robottest(2);
		if(y1>1.5&&y1<3.2&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi))  		
			d(4)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(4)=6;
        end
        %------------------------------------------------------------
        y1=3.2;
		x1=Robottest(1)+(y1-Robottest(2))/tan((Robottest(3)/180)*pi+sonar(i));
		if(x1>6.5&&x1<=7.4&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi))		
			d(5)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(5)=6;
        end
        %------------------------------------------------------------
		x1=6.5;          
        y1=tan((Robottest(3)/180)*pi+sonar(i))*(x1-Robottest(1))+Robottest(2);
		if(y1>3.2&&y1<4.1&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi)) 		
			d(6)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(6)=6;
        end
		%-------------------------------------------------------------
         y1=4.1;
		x1=Robottest(1)+(y1-Robottest(2))/tan((Robottest(3)/180)*pi+sonar(i));
		if(x1>3.3&&x1<=6.5&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi)) 		
			d(7)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(7)=6;
        end
        %-------------------------------------------------------------
        x1=3.3;          
        y1=tan((Robottest(3)/180)*pi+sonar(i))*(x1-Robottest(1))+Robottest(2);
		if(y1>4.1&&y1<=4.7&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi))  
		d(8)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);
		else 
            d(8)=6;
        end
		%--------------------------------------------------------------
        y1=4.7;
		x1=Robottest(1)+(y1-Robottest(2))/tan((Robottest(3)/180)*pi+sonar(i));
		if(x1>0.7&&x1<=3.3&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi)) 
					d(9)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);
		else 
            d(9)=6;
        end
		%--------------------------------------------------------------
        x1=0.7;          
        y1=tan((Robottest(3)/180)*pi+sonar(i))*(x1-Robottest(1))+Robottest(2);
		if(y1>3.4&&y1<=4.7&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi))  		
			d(10)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(10)=6;
        end
		%------------------------------------------------------------
        y1=3.4;
		x1=Robottest(1)+(y1-Robottest(2))/tan((Robottest(3)/180)*pi+sonar(i));
		if(x1>=0&&x1<=0.7&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi))		
			d(11)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(11)=6;
        end
		%-------------------------------------------------------------
		x1=0.0;          
        y1=tan((Robottest(3)/180)*pi+sonar(i))*(x1-Robottest(1))+Robottest(2);
		if(y1>=0&&y1<=3.4&&angle_ok(Robottest(1),Robottest(2),x1,y1,Robottest(3)+sonar(i)*180/pi)) 		
			d(12)=Distance_pointTopoint(Robottest(1),Robottest(2),x1,y1);            
		else 
            d(12)=6;
        end
     %-------------------------------------------------------------- 	 
		for j=1:1:12
			if(d(j)<d(1)) 
                d(1)=d(j);
            end                
        end	
        %-----------------------------------------------------------
		Robottest(i+4)=d(1)+randn(1)/10;      
end   
save('Robottest','Robottest');