Diff: MiniExplorerCoimbra.cpp

Revision:

11:b91fe0ed4fed

Parent:

10:d0109d7cbe7c

Child:

12:1e80471c5c6c

--- a/MiniExplorerCoimbra.cpp	Mon Jul 10 18:03:19 2017 +0000
+++ b/MiniExplorerCoimbra.cpp	Tue Jul 11 10:30:21 2017 +0000
@@ -843,10 +843,10 @@
 	poseKplus1K.data[1][0]=this->yWorld+distanceMovedY;
 	poseKplus1K.data[2][0]=this->thetaWorld+angleMoved;
 
-    pc.printf("\r\n X=%f, Y=%f, theta=%f",poseKplus1K.get(0,0),poseKplus1K.get(1,0),poseKplus1K.get(2,0));
+    pc.printf("\r\n Without correction: X=%f, Y=%f, theta=%f",poseKplus1K.get(0,0),poseKplus1K.get(1,0),poseKplus1K.get(2,0));
     //=====ERROR_MODEL===========================
 
-   	//FP Matrix                     
+   	//FP Matrix slide LocalizationKALMAN                     
     myMatrix Fp(3,3);
     Fp.data[0][0]=1;
     Fp.data[1][1]=1;
@@ -857,7 +857,7 @@
 	myMatrix FpTranspose(3,3);
 	FpTranspose.fillWithTranspose(Fp);
 	
-    //Frl matrix
+    //Frl matrix slide LocalizationKALMAN
     myMatrix Frl(3,2);
     Frl.data[0][0]=0.5*cos(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2));
     Frl.data[0][1]=0.5*cos(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2));
@@ -883,7 +883,7 @@
     Q.data[1][1]=2;
     Q.data[2][2]=0.01;
     
-    //new covariance= Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q
+    //new covariance= Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q slide LocalizationKALMAN
 	myMatrix covariancePositionEstimationKplus1K(3,3);
 	myMatrix temp1(3,3);
 	temp1.fillByMultiplication(Fp,this->covariancePositionEstimationK);//Fp*old covariance
@@ -913,6 +913,18 @@
     float yClosestCellFront;
     float xClosestCellRight;
     float yClosestCellRight;
+    
+    float xSonarLeft=poseKplus1K.data[0][0]+this->sonarLeft.distanceX;
+    float ySonarLeft=poseKplus1K.data[1][0]+this->sonarLeft.distanceY;
+    
+    float xSonarFront=poseKplus1K.data[0][0]+this->sonarFront.distanceX;
+    float ySonarFront=poseKplus1K.data[1][0]+this->sonarFront.distanceY;
+    
+    float xSonarRight=poseKplus1K.data[0][0]+this->sonarRight.distanceX;
+    float ySonarRight=poseKplus1K.data[1][0]+this->sonarRight.distanceY; 
+    
+    //note: sonar.isInRange already incorpore the sonar position and angle relative to the robot
+    
     //get theorical distance to sonar
     for(int i=0;i<this->map.nbCellWidth;i++){
         for(int j=0;j<this->map.nbCellHeight;j++){
@@ -981,21 +993,17 @@
     myMatrix jacobianOfObservationRight(1,3);
     myMatrix jacobianOfObservationFront(1,3);
 
-    float xSonarLeft=poseKplus1K.data[0][0]+this->sonarLeft.distanceX;
-    float ySonarLeft=poseKplus1K.data[1][0]+this->sonarLeft.distanceY;
     //left
     jacobianOfObservationLeft.data[0][0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
     jacobianOfObservationLeft.data[0][1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
     jacobianOfObservationLeft.data[0][2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(poseKplus1K.data[2][0])+ySonarLeft*cos(poseKplus1K.data[2][0]))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(poseKplus1K.data[2][0])+ySonarLeft*sin(poseKplus1K.data[2][0]));
     //front
-    float xSonarFront=poseKplus1K.data[0][0]+this->sonarFront.distanceX;
-    float ySonarFront=poseKplus1K.data[1][0]+this->sonarFront.distanceY;
+    
     jacobianOfObservationFront.data[0][0]=(xSonarFront-xClosestCellFront)/frontCmEstimated;
     jacobianOfObservationFront.data[0][1]=(ySonarFront-yClosestCellFront)/frontCmEstimated;
     jacobianOfObservationFront.data[0][2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(poseKplus1K.data[2][0])+ySonarFront*cos(poseKplus1K.data[2][0]))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(poseKplus1K.data[2][0])+ySonarFront*sin(poseKplus1K.data[2][0]));
     //right
-    float xSonarRight=poseKplus1K.data[0][0]+this->sonarRight.distanceX;
-    float ySonarRight=poseKplus1K.data[1][0]+this->sonarRight.distanceY;
+    
     jacobianOfObservationRight.data[0][0]=(xSonarRight-xClosestCellRight)/rightCmEstimated;
     jacobianOfObservationRight.data[0][1]=(ySonarRight-yClosestCellRight)/rightCmEstimated;
     jacobianOfObservationRight.data[0][2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(poseKplus1K.data[2][0])+ySonarRight*cos(poseKplus1K.data[2][0]))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(poseKplus1K.data[2][0])+ySonarRight*sin(poseKplus1K.data[2][0]));
@@ -1273,7 +1281,6 @@
 			tempCovariancePositionEstimationKplus1Kplus13x2.fillByMultiplication(kalmanGain3X2,compositeInnovationCovariance2x2);
 			tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x2,kalmanGain3X2Transpose);
             covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3);
-        
 			
             break;
         case 110:// front and right