Monte carlo locatization with robot Mikes - Tomas Kubla

Task

We implement MCL-Algorithm (Sebastian THRUN, Wolfram BURGARD, Dieter FOX: PROBABILISTIC ROBOTICS, 1999-2000, page 200)

Model of sensor reading of tag

We were putting robot to many positions in arena and we were logging:

• X position
• Y position
• azimuth
• list of seen tags

When we got data we tried create model of tag

Note: red circle is position of robot, blue cross is position of RFID sensor

Sensor use 4 antennas. Using all antennas sensor see tags around them.

We approximated data with function <math>max(-0.001*abs(x^2 + (2y)^2) + 0.9, 0 )</math>

If sensor is perpendicular to tag it look 2 timer better when sensor is tag parallel

Algorithm

INIT

 int init_mcl(){
   ...
   for(int i = 0; i< HYPO_COUNT; i++){
           hypo[0][i].x = hypo[1][i].x = rand() % 280;
           hypo[0][i].y = hypo[1][i].y = rand() % 280;
           hypo[0][i].alpha = hypo[1][i].alpha = rand() % 360;
           hypo[0][i].w = hypo[1][i].w = 0.3;
           ...
   }
   ...
 }

UPDATE

 int mcl_update(double traveled, int heading){
   ...
   activeHypo = 1-activeHypo;
   ...
   for(int i = 0; i< HYPO_COUNT; i++){
       hypo[1-activeHypo][i].x += traveled * cos(normAlpha(hypo[1-activeHypo][i].alpha+heading)*M_PI/180.0);
       hypo[1-activeHypo][i].y -= traveled * sin(normAlpha(hypo[1-activeHypo][i].alpha+heading)*M_PI/180.0);
       hypo[1-activeHypo][i].alpha = normAlpha(hypo[1-activeHypo][i].alpha + heading);
       if((hypo[1-activeHypo][i].x < 0 )||(280 <hypo[1-activeHypo][i].x)||(hypo[1-activeHypo][i].y < 0)||(280 < hypo[1-activeHypo][i].y)){
         hypo[1-activeHypo][i].w = 0;
         continue;
       }
       //sensor position
       double possx = hypo[1-activeHypo][i].x + cos(hypo[1-activeHypo][i].alpha*M_PI/180.0) * 22;
       double possy = hypo[1-activeHypo][i].y - sin(hypo[1-activeHypo][i].alpha*M_PI/180.0) * 22;
       //tag position
       double minposx;
       double minposy;
       if(rfid_data.ntags == 0){
         ...
         // searing for best tag
         hypo[1-activeHypo][i].w = (1-getp(possx-minposx, possy-minposy)) * hypo[1-activeHypo][i].w / ((1600-636.173)/1600);  
       }else{ 
         // we hope robot see only one tag
           minposx = (rfid_data.x[0]-1)*40;
           minposy = (rfid_data.y[0]-1)*40;
           hypo[1-activeHypo][i].w =    getp(possx-minposx, possy-minposy)  * hypo[1-activeHypo][i].w /(636.173 /1600);  			
       }
       hypo[1-activeHypo][i].w = fmin(hypo[1-activeHypo][i].w, 1);
   }
   ...
   double cumP[HYPO_COUNT];
   double last = 0;
   for(int i = 0; i<= HYPO_COUNT; i++){
           last += hypo[1-activeHypo][i].w;
           cumP[i] = last;
   }
   int i;
   for(i = 0; i < HYPO_COUNT*0.9; i++){
       double next = (double)rand() / (double)RAND_MAX * last;
       for(int j = 0; j< HYPO_COUNT; j++){
           if( next <= cumP[j]){
               hypo[activeHypo][i].x = hypo[1-activeHypo][j].x + generateGaussianNoise(0, 0.03*traveled);
               hypo[activeHypo][i].y = hypo[1-activeHypo][j].y + generateGaussianNoise(0, 0.03*traveled);
               hypo[activeHypo][i].alpha = normAlpha(hypo[1-activeHypo][j].alpha + generateGaussianNoise(0, heading*0.05));
               hypo[activeHypo][i].w = hypo[1-activeHypo][j].w;
               break;
           }	
       }	
   }
  for(; i< HYPO_COUNT; i++){
           hypo[0][i].x = hypo[1][i].x = rand() % 280;
           hypo[0][i].y = hypo[1][i].y = rand() % 280;
           hypo[0][i].alpha = hypo[1][i].alpha = rand() % 360;
           hypo[0][i].w = hypo[1][i].w = 0.01;
   }	
   ...
 }

Source Code

Source code is located on [1]

Some of support scripts and data are here: data.tar

Photo documentation

Experiment 1 Experiment 2