Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

GridBasedLocalisation.hpp@38:5ed7c79fb724, 2017-06-15 (annotated)

Committer:
Ludwigfr
Date:
Thu Jun 15 23:17:55 2017 +0000
Revision:
38:5ed7c79fb724
with some part of lab 4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 38:5ed7c79fb724 1
Ludwigfr 38:5ed7c79fb724 2
Ludwigfr 38:5ed7c79fb724 3 /*
Ludwigfr 38:5ed7c79fb724 4 Lab4
Ludwigfr 38:5ed7c79fb724 5
Ludwigfr 38:5ed7c79fb724 6 make the robot do the same square over and over, see that at one point the errors have accumulated and it is not where the odometria think it is
Ludwigfr 38:5ed7c79fb724 7 solution:
Ludwigfr 38:5ed7c79fb724 8 (here our sensors are bad but our odometrie s okay
Ludwigfr 38:5ed7c79fb724 9 before each new movement we compute an estimation of where we are, compare it to what the robot think (maybe choose our estimate?)
Ludwigfr 38:5ed7c79fb724 10 */
Ludwigfr 38:5ed7c79fb724 11
Ludwigfr 38:5ed7c79fb724 12
Ludwigfr 38:5ed7c79fb724 13
Ludwigfr 38:5ed7c79fb724 14
Ludwigfr 38:5ed7c79fb724 15
Ludwigfr 38:5ed7c79fb724 16
Ludwigfr 38:5ed7c79fb724 17 //for those who passed
Ludwigfr 38:5ed7c79fb724 18 //compute composite innovation
Ludwigfr 38:5ed7c79fb724 19 //compute composite measurement Jacobian
Ludwigfr 38:5ed7c79fb724 20 //compute kalman gain
Ludwigfr 38:5ed7c79fb724 21 //compute updated robot position estimate
Ludwigfr 38:5ed7c79fb724 22 //store the resultant covariance for next estimation
Ludwigfr 38:5ed7c79fb724 23
Ludwigfr 38:5ed7c79fb724 24 //return resultant covariance
Ludwigfr 38:5ed7c79fb724 25
Ludwigfr 38:5ed7c79fb724 26 //compute predicted localisation
Ludwigfr 38:5ed7c79fb724 27 float xEstimatedK=this->xWorld;
Ludwigfr 38:5ed7c79fb724 28 float yEstimatedK=this->yWorld;
Ludwigfr 38:5ed7c79fb724 29 float thetaWorldEstimatedK=this->thetaWorld;
Ludwigfr 38:5ed7c79fb724 30 float distanceMoved;
Ludwigfr 38:5ed7c79fb724 31 float angleMoved;
Ludwigfr 38:5ed7c79fb724 32
Ludwigfr 38:5ed7c79fb724 33 float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved);
Ludwigfr 38:5ed7c79fb724 34 float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved);
Ludwigfr 38:5ed7c79fb724 35 float thetaEstimatedKNext=this->thetaWorld+angleMoved;
Ludwigfr 38:5ed7c79fb724 36 //check measurements from sonars, see if they passed the validation gate
Ludwigfr 38:5ed7c79fb724 37 float leftCm = get_distance_left_sensor()/10;
Ludwigfr 38:5ed7c79fb724 38 float frontCm = get_distance_front_sensor()/10;
Ludwigfr 38:5ed7c79fb724 39 float rightCm = get_distance_right_sensor()/10;
Ludwigfr 38:5ed7c79fb724 40 //if superior to sonar range, put the value to sonar max range + 1
Ludwigfr 38:5ed7c79fb724 41 if(leftCm > this->sonarLeft.maxRange)
Ludwigfr 38:5ed7c79fb724 42 leftCm=this->sonarLeft.maxRange+1;
Ludwigfr 38:5ed7c79fb724 43 if(frontCm > this->sonarFront.maxRange)
Ludwigfr 38:5ed7c79fb724 44 frontCm=this->sonarFront.maxRange+1;
Ludwigfr 38:5ed7c79fb724 45 if(rightCm > this->sonarRight.maxRange)
Ludwigfr 38:5ed7c79fb724 46 rightCm=this->sonarRight.maxRange+1;
Ludwigfr 38:5ed7c79fb724 47 //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
Ludwigfr 38:5ed7c79fb724 48 float leftCmEstimated=this->sonarLeft.maxRange+1;
Ludwigfr 38:5ed7c79fb724 49 float frontCmEstimated=this->sonarFront.maxRange+1;
Ludwigfr 38:5ed7c79fb724 50 float rightCmEstimated=this->sonarRight.maxRange+1;
Ludwigfr 38:5ed7c79fb724 51 float xWorldCell;
Ludwigfr 38:5ed7c79fb724 52 float yWorldCell;
Ludwigfr 38:5ed7c79fb724 53 float currDistance;
Ludwigfr 38:5ed7c79fb724 54 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 38:5ed7c79fb724 55 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 38:5ed7c79fb724 56 //check if occupied, if not discard
Ludwigfr 38:5ed7c79fb724 57 if(this->map.get_proba_cell(i,j)==1){
Ludwigfr 38:5ed7c79fb724 58 //check if in sonar range
Ludwigfr 38:5ed7c79fb724 59 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 38:5ed7c79fb724 60 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 38:5ed7c79fb724 61 //check left
Ludwigfr 38:5ed7c79fb724 62 currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);
Ludwigfr 38:5ed7c79fb724 63 if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1)
Ludwigfr 38:5ed7c79fb724 64 //check if distance is lower than previous, update lowest if so
Ludwigfr 38:5ed7c79fb724 65 if(currDistance < leftCmEstimated)
Ludwigfr 38:5ed7c79fb724 66 leftCmEstimated= currDistance;
Ludwigfr 38:5ed7c79fb724 67 }
Ludwigfr 38:5ed7c79fb724 68 //check front
Ludwigfr 38:5ed7c79fb724 69 currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);
Ludwigfr 38:5ed7c79fb724 70 if((currDistance < this->sonarFront.maxRange) && currDistance!=-1)
Ludwigfr 38:5ed7c79fb724 71 //check if distance is lower than previous, update lowest if so
Ludwigfr 38:5ed7c79fb724 72 if(currDistance < frontCmEstimated)
Ludwigfr 38:5ed7c79fb724 73 frontCmEstimated= currDistance;
Ludwigfr 38:5ed7c79fb724 74 }
Ludwigfr 38:5ed7c79fb724 75 //check right
Ludwigfr 38:5ed7c79fb724 76 currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);
Ludwigfr 38:5ed7c79fb724 77 if((currDistance < this->sonarRight.maxRange) && currDistance!=-1)
Ludwigfr 38:5ed7c79fb724 78 //check if distance is lower than previous, update lowest if so
Ludwigfr 38:5ed7c79fb724 79 if(currDistance < rightCmEstimated)
Ludwigfr 38:5ed7c79fb724 80 rightCmEstimated= currDistance;
Ludwigfr 38:5ed7c79fb724 81 }
Ludwigfr 38:5ed7c79fb724 82 }
Ludwigfr 38:5ed7c79fb724 83 }
Ludwigfr 38:5ed7c79fb724 84 }
Ludwigfr 38:5ed7c79fb724 85 //get the innoncation: the value of the difference between the actual measure and what we anticipated
Ludwigfr 38:5ed7c79fb724 86 float innovationLeft=leftCm-leftCmEstimated;
Ludwigfr 38:5ed7c79fb724 87 float innovationFront=frontCm-frontCmEstimated;
Ludwigfr 38:5ed7c79fb724 88 float innovationRight=-rightCm-rightCmEstimated;
Ludwigfr 38:5ed7c79fb724 89 //compute jacobian of observation
Ludwigfr 38:5ed7c79fb724 90 float jacobianOfObservationLeft[3];
Ludwigfr 38:5ed7c79fb724 91 float jacobianOfObservationRight[3];
Ludwigfr 38:5ed7c79fb724 92 float jacobianOfObservationFront[3];
Ludwigfr 38:5ed7c79fb724 93 jacobianOfObservationLeft[0]=()/leftCmEstimated
Ludwigfr 38:5ed7c79fb724 94 jacobianOfObservationLeft[1]=
Ludwigfr 38:5ed7c79fb724 95 jacobianOfObservationLeft[2]=
Ludwigfr 38:5ed7c79fb724 96 //check if it pass the validation gate
Ludwigfr 38:5ed7c79fb724 97 bool leftPassed=false;
Ludwigfr 38:5ed7c79fb724 98 bool frontPassed=false;
Ludwigfr 38:5ed7c79fb724 99 bool rightPassed=false;
Ludwigfr 38:5ed7c79fb724 100
Ludwigfr 38:5ed7c79fb724 101
Ludwigfr 38:5ed7c79fb724 102