with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: GridBasedLocalisation.hpp
- Revision:
- 38:5ed7c79fb724
diff -r b4c45e43ad29 -r 5ed7c79fb724 GridBasedLocalisation.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GridBasedLocalisation.hpp Thu Jun 15 23:17:55 2017 +0000 @@ -0,0 +1,102 @@ + + +/* +Lab4 + +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 +solution: +(here our sensors are bad but our odometrie s okay +before each new movement we compute an estimation of where we are, compare it to what the robot think (maybe choose our estimate?) +*/ + + + + + + +//for those who passed +//compute composite innovation +//compute composite measurement Jacobian +//compute kalman gain +//compute updated robot position estimate +//store the resultant covariance for next estimation + +//return resultant covariance + +//compute predicted localisation +float xEstimatedK=this->xWorld; +float yEstimatedK=this->yWorld; +float thetaWorldEstimatedK=this->thetaWorld; +float distanceMoved; +float angleMoved; + +float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved); +float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved); +float thetaEstimatedKNext=this->thetaWorld+angleMoved; +//check measurements from sonars, see if they passed the validation gate +float leftCm = get_distance_left_sensor()/10; +float frontCm = get_distance_front_sensor()/10; +float rightCm = get_distance_right_sensor()/10; +//if superior to sonar range, put the value to sonar max range + 1 +if(leftCm > this->sonarLeft.maxRange) + leftCm=this->sonarLeft.maxRange+1; +if(frontCm > this->sonarFront.maxRange) + frontCm=this->sonarFront.maxRange+1; +if(rightCm > this->sonarRight.maxRange) + rightCm=this->sonarRight.maxRange+1; +//get the estimated measure we should have according to our knowledge of the map and the previously computed localisation +float leftCmEstimated=this->sonarLeft.maxRange+1; +float frontCmEstimated=this->sonarFront.maxRange+1; +float rightCmEstimated=this->sonarRight.maxRange+1; +float xWorldCell; +float yWorldCell; +float currDistance; +for(int i=0;i<this->map.nbCellWidth;i++){ + for(int j=0;j<this->map.nbCellHeight;j++){ + //check if occupied, if not discard + if(this->map.get_proba_cell(i,j)==1){ + //check if in sonar range + xWorldCell=this->map.cell_width_coordinate_to_world(i); + yWorldCell=this->map.cell_height_coordinate_to_world(j); + //check left + currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext); + if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1) + //check if distance is lower than previous, update lowest if so + if(currDistance < leftCmEstimated) + leftCmEstimated= currDistance; + } + //check front + currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext); + if((currDistance < this->sonarFront.maxRange) && currDistance!=-1) + //check if distance is lower than previous, update lowest if so + if(currDistance < frontCmEstimated) + frontCmEstimated= currDistance; + } + //check right + currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext); + if((currDistance < this->sonarRight.maxRange) && currDistance!=-1) + //check if distance is lower than previous, update lowest if so + if(currDistance < rightCmEstimated) + rightCmEstimated= currDistance; + } + } + } +} +//get the innoncation: the value of the difference between the actual measure and what we anticipated +float innovationLeft=leftCm-leftCmEstimated; +float innovationFront=frontCm-frontCmEstimated; +float innovationRight=-rightCm-rightCmEstimated; +//compute jacobian of observation +float jacobianOfObservationLeft[3]; +float jacobianOfObservationRight[3]; +float jacobianOfObservationFront[3]; +jacobianOfObservationLeft[0]=()/leftCmEstimated +jacobianOfObservationLeft[1]= +jacobianOfObservationLeft[2]= +//check if it pass the validation gate +bool leftPassed=false; +bool frontPassed=false; +bool rightPassed=false; + + +