![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
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?
User | Revision | Line number | New 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 |