test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
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