Revision 39:890439b495e3, committed 2017-06-16

Comitter:

Ludwigfr

Date:

Fri Jun 16 10:40:53 2017 +0000

Parent:

38:5ed7c79fb724

Commit message:

last version with the 4th lab;

Changed in this revision

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

--- a/Map.cpp	Thu Jun 15 23:17:55 2017 +0000
+++ b/Map.cpp	Fri Jun 16 10:40:53 2017 +0000
@@ -33,6 +33,14 @@
     }
 }
 
+void Map::fill_map_with_initial(){
+	for (int i = 0; i<this->nbCellWidth; i++) {
+        for (int j = 0; j<this->nbCellHeight; j++) {
+            this->cellsLogValues[i][j] = initialLogValues[i][j];
+        }
+    }
+}
+
 //returns the probability [0,1] that the cell is occupied from the log valAue lt
 float Map::log_to_proba(float lt){
     return 1-1/(1+exp(lt));

--- a/Map.hpp	Thu Jun 15 23:17:55 2017 +0000
+++ b/Map.hpp	Fri Jun 16 10:40:53 2017 +0000
@@ -46,6 +46,8 @@
     
     float get_proba_cell(int widthIndice, int heightIndice);
     
+    void fill_map_with_initial();
+    
     //Updates map value
     void update_cell_value(int widthIndice,int heightIndice ,float proba);
         

--- a/MiniExplorerCoimbra.cpp	Thu Jun 15 23:17:55 2017 +0000
+++ b/MiniExplorerCoimbra.cpp	Fri Jun 16 10:40:53 2017 +0000
@@ -540,4 +540,570 @@
     }
     return angleBeetweenRobotAndTarget-theta;
 }
-*/
\ No newline at end of file
+*/
+
+   
+/*
+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?)
+*/
+
+void MiniExplorerCoimbra::test_procedure_lab_4(){
+    this->map.fill_map_with_initial();
+    float xEstimatedK=this->xWorld;
+    float yEstimatedK=this->yWorld;
+    float thetaWorldEstimatedK=this->thetaWorld;
+    float distanceMoved;
+    float angleMoved;
+    float PreviousCovarianceOdometricPositionEstimate[3][3];
+    PreviousCovarianceOdometricPositionEstimate[0][0]=0;
+    PreviousCovarianceOdometricPositionEstimate[0][1]=0;
+    PreviousCovarianceOdometricPositionEstimate[0][2]=0;
+    PreviousCovarianceOdometricPositionEstimate[1][0]=0;
+    PreviousCovarianceOdometricPositionEstimate[1][1]=0;
+    PreviousCovarianceOdometricPositionEstimate[1][2]=0;
+    PreviousCovarianceOdometricPositionEstimate[2][0]=0;
+    PreviousCovarianceOdometricPositionEstimate[2][1]=0;
+    PreviousCovarianceOdometricPositionEstimate[2][2]=0;
+    while(1){
+        distanceMoved=50;
+        angleMoved=0;
+        this->go_straight_line(50);
+        wait(1);
+        procedure_lab_4(xEstimatedK,yEstimatedK,thetaEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
+        pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
+        pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaEstimatedK);
+        this->turn_to_target(PI/2);
+        distanceMoved=0;
+        angleMoved=PI/2;
+        procedure_lab_4(xEstimatedK,yEstimatedK,thetaEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
+        pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
+        pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaEstimatedK);
+    }
+}
+
+
+//compute predicted localisation (assume movements are either straight lines or pure rotation), update PreviousCovarianceOdometricPositionEstimate
+//TODO tweak constant rewritte it good
+void MiniExplorerCoimbra::procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]){
+
+    float distanceMovedByLeftWheel=distanceMoved/2;
+    float distanceMovedByRightWheel=distanceMoved/2;
+    if(angleMoved !=0){
+        //TODO check that not sure
+        distanceMovedByLeftWheel=-angleMoved/(2*this->distanceWheels);
+        distanceMovedByRightWheel=angleMoved/(2*this->distanceWheels);
+    }else{
+        distanceMovedByLeftWheel=distanceMoved/2;
+        distanceMovedByRightWheel=distanceMoved/2;
+    }
+        
+    float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved);
+    float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved);
+    float thetaEstimatedKNext=thetaWorldEstimatedK+angleMoved;
+    
+    float KRight=0.1;//error constant 
+    float KLEft=0.1;//error constant 
+    float motionIncrementCovarianceMatrix[2][2];
+    motionIncrementCovarianceMatrix[0][0]=kRight*abs(distanceMovedRight);
+    motionIncrementCovarianceMatrix[0][1]=0;
+    motionIncrementCovarianceMatrix[1][0]=0;
+    motionIncrementCovarianceMatrix[1][1]=kLeft*abs(distanceMovedLeft);
+    
+    float jacobianFp[3][3];
+    jacobianFp[0][0]=1;
+    jacobianFp[0][1]=0;
+    jacobianFp[0][2]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFp[1][0]=0;
+    jacobianFp[1][1]=1;
+    jacobianFp[1][2]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);;
+    jacobianFp[2][0]=0;
+    jacobianFp[2][1]=0;
+    jacobianFp[2][2]=1;
+    
+    float jacobianFpTranspose[3][3];
+    jacobianFpTranspose[0][0]=1;
+    jacobianFpTranspose[0][1]=0;
+    jacobianFpTranspose[0][2]=0;
+    jacobianFpTranspose[1][0]=0;
+    jacobianFpTranspose[1][1]=1;
+    jacobianFpTranspose[1][2]=0;
+    jacobianFpTranspose[2][0]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFpTranspose[2][1]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFpTranspose[2][2]=1;
+    
+    float jacobianFDeltarl[3][2];
+    jacobianFDeltarl[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFDeltarl[0][1]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFDeltarl[1][0]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFDeltarl[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFDeltarl[2][0]=1/this->distanceWheels;
+    jacobianFDeltarl[2][1]=-1/this->distanceWheels;
+    
+    float jacobianFDeltarlTranspose[2][3];//1,3,5;2,4,6
+    jacobianFDeltarlTranspose[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFDeltarlTranspose[0][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFDeltarlTranspose[0][2]=1/this->distanceWheels;
+    jacobianFDeltarlTranspose[1][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFDeltarlTranspose[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
+    jacobianFDeltarlTranspose[1][2]=-1/this->distanceWheels;
+    
+    float tempMatrix3_3[3][3];
+    for(i = 0; i < 3; ++i){//mult 3*3, 3*3
+        for(j = 0; j < 3; ++j){
+            for(k = 0; k < 3; ++k){
+                tempMatrix3_3[i][j] += jacobianFp[i][k] * PreviousCovarianceOdometricPositionEstimate[k][j];
+            }
+        }
+    }
+    float sndTempMatrix3_3[3][3];
+    for(i = 0; i < 3; ++i){//mult 3*3, 3*3
+        for(j = 0; j < 3; ++j){
+            for(k = 0; k < 3; ++k){
+                sndTempMatrix3_3[i][j] += tempMatrix3_3[i][k] * jacobianFpTranspose[k][j];
+            }
+        }
+    }
+    float tempMatrix3_2[3][2];
+    for(i = 0; i < 3; ++i){//mult 3*2 , 2,2
+        for(j = 0; j < 2; ++j){
+            for(k = 0; k < 2; ++k){
+                tempMatrix3_2[i][j] += jacobianFDeltarl[i][k] * motionIncrementCovarianceMatrix[k][j];
+            }
+        }
+    }
+    float thrdTempMatrix3_3[3][3];
+    for(i = 0; i < 3; ++i){//mult 3*2 , 2,3
+        for(j = 0; j < 3; ++j){
+            for(k = 0; k < 2; ++k){
+                thrdTempMatrix3_3[i][j] += tempMatrix3_2[i][k] * jacobianFDeltarlTranspose[k][j];
+            }
+        }
+    }
+    float newCovarianceOdometricPositionEstimate[3][3];
+    for(i = 0; i < 3; ++i){//add 3*3 , 3*3
+        for(j = 0; j < 3; ++j){
+            newCovarianceOdometricPositionEstimate[i][j]=sndTempMatrix3_3[i][j]+thrdTempMatrix3_3[i][j];
+        }
+    }
+    
+    //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;
+    if(frontCm > this->sonarFront.maxRange)
+        frontCm=this->sonarFront.maxRange;
+    if(rightCm > this->sonarRight.maxRange)
+        rightCm=this->sonarRight.maxRange;
+    //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
+    float leftCmEstimated=this->sonarLeft.maxRange;
+    float frontCmEstimated=this->sonarFront.maxRange;
+    float rightCmEstimated=this->sonarRight.maxRange;
+    float xWorldCell;
+    float yWorldCell;
+    float currDistance;
+    float xClosetCellLeft;
+    float yClosetCellLeft;
+    float xClosetCellFront;
+    float yClosetCellFront;
+    float xClosetCellRight;
+    float yClosetCellRight;
+    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;
+                        xClosetCellLeft=xWorldCell;
+                        yClosetCellLeft=yWorldCell;
+                    }
+                }
+                //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;
+                        xClosetCellFront=xWorldCell;
+                        yClosetCellFront=yWorldCell;
+                    }
+                }
+                //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;
+                        xClosetCellRight=xWorldCell;
+                        yClosetCellRight=yWorldCell;
+                    }
+                }
+            }
+        }
+    }
+    //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];
+    float xSonarLeft=xEstimatedKNext+this->sonarLeft.distanceX;
+    float ySonarLeft=yEstimatedKNext+this->sonarLeft.distanceY;
+    //left
+    jacobianOfObservationLeft[0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
+    jacobianOfObservationLeft[1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
+    jacobianOfObservationLeft[2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaEstimatedKNext)+ySonarLeft*cos(thetaEstimatedKNext)+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaEstimatedKNext)+ySonarLeft*sin(thetaEstimatedKNext));
+    //front
+    float xSonarFront=xEstimatedKNext+this->sonarFront.distanceX;
+    float ySonarFront=yEstimatedKNext+this->sonarFront.distanceY;
+    jacobianOfObservationFront[0]=(xSonarFront-xClosestCellFront)/FrontCmEstimated;
+    jacobianOfObservationFront[1]=(ySonarFront-yClosestCellFront)/FrontCmEstimated;
+    jacobianOfObservationFront[2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaEstimatedKNext)+ySonarFront*cos(thetaEstimatedKNext)+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaEstimatedKNext)+ySonarFront*sin(thetaEstimatedKNext));
+    //right
+    float xSonarRight=xEstimatedKNext+this->sonarRight.distanceX;
+    float ySonarRight=yEstimatedKNext+this->sonarRight.distanceY;
+    jacobianOfObservationRight[0]=(xSonarRight-xClosestCellRight)/RightCmEstimated;
+    jacobianOfObservationRight[1]=(ySonarRight-yClosestCellRight)/RightCmEstimated;
+    jacobianOfObservationRight[2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaEstimatedKNext)+ySonarRight*cos(thetaEstimatedKNext)+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaEstimatedKNext)+ySonarRight*sin(thetaEstimatedKNext));
+    
+    float innovationCovarianceLeft[3][3];
+    float innovationCovarianceFront[3][3];
+    float innovationCovarianceRight[3][3];
+    //left
+    float TempMatrix3_3InnovationLeft[3];
+    TempMatrix3_3InnovationLeft[0]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][0];
+    TempMatrix3_3InnovationLeft[1]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][1];
+    TempMatrix3_3InnovationLeft[2]jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][2];
+    float innovationConvarianceLeft=TempMatrix3_3InnovationLeft[0]*jacobianOfObservationLeft[0]+TempMatrix3_3InnovationLeft[1]*jacobianOfObservationLeft[1]+TempMatrix3_3InnovationLeft[2]*jacobianOfObservationLeft[2];
+    //front
+    float TempMatrix3_3InnovationFront[3];
+    TempMatrix3_3InnovationFront[0]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][0];
+    TempMatrix3_3InnovationFront[1]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][1];
+    TempMatrix3_3InnovationFront[2]jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][2];
+    float innovationConvarianceFront=TempMatrix3_3InnovationFront[0]*jacobianOfObservationFront[0]+TempMatrix3_3InnovationFront[1]*jacobianOfObservationFront[1]+TempMatrix3_3InnovationFront[2]*jacobianOfObservationFront[2];
+    //right
+    float TempMatrix3_3InnovationRight[3];
+    TempMatrix3_3InnovationRight[0]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][0];
+    TempMatrix3_3InnovationRight[1]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][1];
+    TempMatrix3_3InnovationRight[2]jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][2];
+    float innovationConvarianceRight=TempMatrix3_3InnovationRight[0]*jacobianOfObservationRight[0]+TempMatrix3_3InnovationRight[1]*jacobianOfObservationRight[1]+TempMatrix3_3InnovationRight[2]*jacobianOfObservationRight[2];
+    
+    //check if it pass the validation gate 
+    float gateScoreLeft=innovationLeft*innovationLeft/innovationConvarianceLeft;
+    float gateScoreFront=innovationFront*innovationFront/innovationConvarianceFront;
+    float gateScoreRight=innovationRight*innovationRight/innovationConvarianceRight;
+    int leftPassed=0;
+    int frontPassed=0;
+    int rightPassed=0;
+    int e=10;//constant 
+    if(gateScoreLeft < e)
+        leftPassed=1;
+    if(gateScoreFront < e)
+        frontPassed=10;
+    if(gateScoreRight < e)
+        rightPassed=100;
+    //for those who passed
+    //compute composite innovation
+    float compositeInnovation[3];
+    int nbPassed=leftPassed+frontPassed+rightPassed;
+    switch(nbPassed){
+        case 0://none
+            compositeInnovation[0]=0;
+            compositeInnovation[1]=0;
+            compositeInnovation[2]=0;
+            break;
+        case 1://left
+            compositeInnovation[0]=innovationLeft;
+            compositeInnovation[1]=0;
+            compositeInnovation[2]=0;
+            break;
+        case 10://front
+            compositeInnovation[0]=0;
+            compositeInnovation[1]=innovationFront;
+            compositeInnovation[2]=0;
+            break;
+        case 11://left and front
+            compositeInnovation[0]=innovationLeft;
+            compositeInnovation[1]=innovationFront;
+            compositeInnovation[2]=0;
+            break;
+        case 100://right
+            compositeInnovation[0]=0;
+            compositeInnovation[1]=0;
+            compositeInnovation[2]=innovationRight;
+            break;
+        case 101://right and left
+            compositeInnovation[0]=innovationLeft;
+            compositeInnovation[1]=0;
+            compositeInnovation[2]=innovationRight;
+            break;
+        case 110://right and front
+            compositeInnovation[0]=0;
+            compositeInnovation[1]=innovationFront;
+            compositeInnovation[2]=innovationRight;
+            break
+        case 111://right front and left
+            compositeInnovation[0]=innovationLeft;
+            compositeInnovation[1]=innovationFront;
+            compositeInnovation[2]=innovationRight;
+            break;
+    }
+    //compute composite measurement Jacobian
+    switch(nbPassed){
+        case 0://none
+            break;
+        case 1://left
+            //compositeMeasurementJacobian = jacobianOfObservationLeft
+            break;
+        case 10://front
+            //compositeMeasurementJacobian = jacobianOfObservationFront
+            break;
+        case 11://left and front
+            float compositeMeasurementJacobian[6]
+            compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
+            compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
+            compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
+            compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
+            compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
+            compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
+            break;
+        case 100://right
+            //compositeMeasurementJacobian = jacobianOfObservationRight
+            break;
+        case 101://right and left
+            float compositeMeasurementJacobian[6]
+            compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
+            compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
+            compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
+            compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
+            compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
+            compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
+    
+            break;
+        case 110://right and front
+            float compositeMeasurementJacobian[6]
+            compositeMeasurementJacobian[0]= jacobianOfObservationFront[0];
+            compositeMeasurementJacobian[1]= jacobianOfObservationFront[1];
+            compositeMeasurementJacobian[2]= jacobianOfObservationFront[2];
+            compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
+            compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
+            compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
+    
+            break
+        case 111://right front and left
+            float compositeMeasurementJacobian[9]
+            compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
+            compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
+            compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
+            compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
+            compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
+            compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
+            compositeMeasurementJacobian[6]= jacobianOfObservationRight[0];
+            compositeMeasurementJacobian[7]= jacobianOfObservationRight[1];
+            compositeMeasurementJacobian[8]= jacobianOfObservationRight[2];
+            break;
+    }
+    //compute composite innovation covariance TODO dont have time, I assume you can just multiply them, can also simply all this easily
+    float compositeInnovationCovariance
+    switch(nbPassed){
+        case 0://none
+            compositeInnovationCovariance=1;
+            break;
+        case 1://left
+            compositeInnovationCovariance = innovationConvarianceLeft;
+            
+            break;
+        case 10://front
+            compositeInnovationCovariance = innovationConvarianceFront;
+            break;
+        case 11://left and front
+            compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront;
+    
+            break;
+        case 100://right
+            compositeInnovationCovariance = innovationConvarianceRight;
+            break;
+        case 101://right and left
+            compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceRight;
+    
+            break;
+        case 110://right and front
+            compositeInnovationCovariance=innovationConvarianceFront*innovationConvarianceRight;
+    
+    
+            break
+        case 111://right front and left
+            compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront*innovationConvarianceRight;
+      
+            break;
+    }
+    
+    //compute kalman gain
+    switch(nbPassed){
+        //mult nbSonarPassed*3 , 3*3
+        case 0://none
+            break;
+        case 1://left
+            float kalmanGain[3][3];
+            for(i = 0; i < 3; ++i){//mult 3*3, 3*1
+                for(j = 0; j < 1; ++j){
+                    for(k = 0; k < 3; ++k){
+                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
+                    }
+                }
+            }
+            
+            break;
+        case 10://front
+            float kalmanGain[3][3];
+            for(i = 0; i < 3; ++i){//mult 3*3, 3*1
+                for(j = 0; j < 1; ++j){
+                    for(k = 0; k < 3; ++k){
+                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
+                    }
+                }
+            }
+            break;
+        case 11://left and front
+            float kalmanGain[3][3];
+            for(i = 0; i < 3; ++i){//mult 3*3, 3*2
+                for(j = 0; j < 2; ++j){
+                    for(k = 0; k < 3; ++k){
+                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
+                    }
+                }
+            }
+    
+            break;
+        case 100://right
+            float kalmanGain[3][3];
+            for(i = 0; i < 3; ++i){//mult 3*3, 3*1
+                for(j = 0; j < 1; ++j){
+                    for(k = 0; k < 3; ++k){
+                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
+                    }
+                }
+            }
+            break;
+        case 101://right and left
+            float kalmanGain[3][3];
+            for(i = 0; i < 3; ++i){//mult 3*3, 3*2
+                for(j = 0; j < 2; ++j){
+                    for(k = 0; k < 3; ++k){
+                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
+                    }
+                }
+            }
+    
+            break;
+        case 110://right and front
+            float kalmanGain[3][3];
+            for(i = 0; i < 3; ++i){//mult 3*3, 3*2
+                for(j = 0; j < 2; ++j){
+                    for(k = 0; k < 3; ++k){
+                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
+                    }
+                }
+            }
+    
+    
+            break
+        case 111://right front and left
+            float kalmanGain[3][3];
+            for(i = 0; i < 3; ++i){//mult 3*3, 3*3
+                for(j = 0; j < 3; ++j){
+                    for(k = 0; k < 3; ++k){
+                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
+                    }
+                }
+            }
+      
+            break;
+    }
+    for(i = 0; i < 3; ++i){//mult 3*3, 3*3
+        for(j = 0; j < 3; ++j){
+            kalmanGain[i][j] = kalmanGain[i][j][i][k]/compositeInnovationCovariance;
+        }
+    }
+    //compute kalman gain * composite innovation
+    //mult 3*3 , 3*1
+    float result3_1[3][1];
+    for(i = 0; i < 3; ++i){//mult 3*3, 3*1
+        for(j = 0; j < 1; ++j){
+            for(k = 0; k < 3; ++k){
+                result3_1[i][j] += kalmanGain[i][k] * compositeInnovation[k];
+            }
+        }
+    }
+    //compute updated robot position estimate
+    float xEstimatedKLast=xEstimatedKNext+result3_1[0];
+    float yEstimatedKLast=yEstimatedKNext+result3_1[1];
+    float thetaEstimatedKLast=thetaEstimatedKNext+result3_1[2];
+    //store the resultant covariance for next estimation
+    /*
+    a b c
+    d e f
+    g h i
+    
+    a d g
+    b e h
+    c f i
+    */
+    float kalmanGainTranspose[3][3];
+    kalmanGainTranspose[0][0]=kalmanGain[0][0];
+    kalmanGainTranspose[0][1]=kalmanGain[1][0];
+    kalmanGainTranspose[0][2]=kalmanGain[2][0];
+    kalmanGainTranspose[1][0]=kalmanGain[0][1];
+    kalmanGainTranspose[1][1]=kalmanGain[1][1];
+    kalmanGainTranspose[1][2]=kalmanGain[2][1];
+    kalmanGainTranspose[2][0]=kalmanGain[0][2];
+    kalmanGainTranspose[2][1]=kalmanGain[1][2];
+    kalmanGainTranspose[2][2]=kalmanGain[2][2];
+    
+    //mult 3*3 , 3*3
+    float fithTempMatrix3_3[3][3];
+    for(i = 0; i < 3; ++i){//mult 3*3 , 3*3
+        for(j = 0; j < 3; ++j){
+            for(k = 0; k < 3; ++k){
+                fithTempMatrix3_3[i][j] += kalmanGain[i][k] * kalmanGainTranspose[k][j];
+            }
+        }
+    }
+    for(i = 0; i < 3; ++i){//add 3*3 , 3*3
+        for(j = 0; j < 3; ++j){
+            fithTempMatrix3_3[i][j]=fithTempMatrix3_3[i][j]*compositeInnovationCovariance;
+        }
+    }
+    float covariancePositionEsimatedLast[3][3];
+    for(i = 0; i < 3; ++i){//add 3*3 , 3*3
+        for(j = 0; j < 3; ++j){
+            covariancePositionEsimatedLast[i][j]=fithTempMatrix3_3[i][j]+newCovarianceOdometricPositionEstimate[i][j];
+        }
+    }
+    //update PreviousCovarianceOdometricPositionEstimate
+    for(i = 0; i < 3; ++i){
+        for(j = 0; j < 3; ++j){
+            PreviousCovarianceOdometricPositionEstimate[i][j]=covariancePositionEsimatedLast[i][j];
+        }
+    }
+    
+    xEstimatedK=xEstimatedKLast;
+    yEstimatedK=yEstimatedKLast;
+    thetaEstimatedK=thetaEstimatedKLast;
+}

--- a/MiniExplorerCoimbra.hpp	Thu Jun 15 23:17:55 2017 +0000
+++ b/MiniExplorerCoimbra.hpp	Fri Jun 16 10:40:53 2017 +0000
@@ -5,9 +5,6 @@
 #include "Sonar.hpp"
 #include<math.h>
 
-
-
-
 	/*
 Robot coordinate system:      			World coordinate system:
       ^                 							^
@@ -71,6 +68,10 @@
 	
 	void vff(bool* reached, float targetXWorld, float targetYWorld);
 	
+	void procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]){
+
+	void test_procedure_lab_4();
+	
 	//compute the force on X and Y
 	void compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld);