Difference between revisions of "Monte carlo locatization with robot Mikes - Tomas Kubla"
From RoboWiki
(New page: == Task == We had to implement MCL-Algorithm (Sebastian THRUN, Wolfram BURGARD, Dieter FOX: PROBABILISTIC ROBOTICS, 1999-2000, page 200) == Creating tag model == We were putting robot to...) |
m |
||
(14 intermediate revisions by the same user not shown) | |||
Line 1: | Line 1: | ||
== Task == | == Task == | ||
− | We | + | 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: | We were putting robot to many positions in arena and we were logging: | ||
Line 10: | Line 10: | ||
* list of seen tags | * list of seen tags | ||
+ | When we got data we tried create model of tag | ||
− | + | [[Image:raw.png|500px]] | |
+ | 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. | ||
+ | |||
+ | [[Image:gain.png]] | ||
+ | |||
+ | We approximated data with function | ||
+ | <math>max(-0.001*abs(x^2 + (2y)^2) + 0.9, 0 )</math> | ||
+ | |||
+ | [[Image:getp.png]] | ||
+ | |||
+ | If sensor is perpendicular to tag it look 2 timer better when sensor is tag parallel | ||
+ | |||
+ | == Algorithm == | ||
+ | |||
+ | INIT | ||
<code> | <code> | ||
int init_mcl(){ | int init_mcl(){ | ||
Line 27: | Line 44: | ||
</code> | </code> | ||
+ | UPDATE | ||
+ | <code> | ||
+ | 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; | ||
+ | } | ||
+ | ... | ||
+ | } | ||
+ | </code> | ||
== Source Code == | == Source Code == | ||
Line 32: | Line 110: | ||
Some of support scripts and data are here: | Some of support scripts and data are here: | ||
− | [[Media:data. | + | [[Media:data.tar|data.tar]] |
+ | |||
+ | == Photo documentation == | ||
+ | |||
+ | [[Experiment 1]] | ||
+ | [[Experiment 2]] | ||
== Video documentation == | == Video documentation == | ||
− | <youtube> | + | <youtube>zfyK7YjYMSE</youtube> |
− | |||
− | |||
− | + | __notoc__ |
Latest revision as of 06:28, 20 February 2017
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
Video documentation