Diff: GridBasedLocalisation.hpp

Revision:

39:890439b495e3

Parent:

38:5ed7c79fb724

--- 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;
-
-
-