Diff: GridBasedLocalisation.hpp

Revision:

38:5ed7c79fb724

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