with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: GridBasedLocalisation.hpp
- Revision:
- 39:890439b495e3
- Parent:
- 38:5ed7c79fb724
diff -r 5ed7c79fb724 -r 890439b495e3 GridBasedLocalisation.hpp --- a/GridBasedLocalisation.hpp Thu Jun 15 23:17:55 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,102 +0,0 @@ - - -/* -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; - - -