test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 10:d0109d7cbe7c
- Parent:
- 9:1cc27f33d3e1
- Child:
- 11:b91fe0ed4fed
diff -r 1cc27f33d3e1 -r d0109d7cbe7c MiniExplorerCoimbra.cpp --- a/MiniExplorerCoimbra.cpp Mon Jul 10 16:23:52 2017 +0000 +++ b/MiniExplorerCoimbra.cpp Mon Jul 10 18:03:19 2017 +0000 @@ -759,50 +759,10 @@ //4th LAB //starting position lower left -void MiniExplorerCoimbra::test_procedure_lab_4(float sizeX, float sizeY, int nbRectangle){ +void MiniExplorerCoimbra::test_procedure_lab_4(float sizeX, float sizeY){ this->map.fill_map_with_kalman_knowledge(); - - this->go_to_point_with_angle_kalman(this->xWorld+sizeX,this->yWorld,this->thetaWorld); - - /* - for(int j=0;j<nbRectangle;j++){ - //right - this->go_to_point_with_angle_kalman(this->xWorld+sizeX,this->yWorld,this->thetaWorld); - this->go_turn_kalman(this->xWorld,this->yWorld,this->thetaWorld+PI/2); - - this->print_map_with_robot_position(); - pc.printf("\n\rX= %f",this->xWorld); - pc.printf("\n\rY= %f",this->yWorld); - pc.printf("\n\rtheta= %f",this->thetaWorld); - - //up - this->go_to_point_with_angle_kalman(this->xWorld+sizeX,this->yWorld+sizeY,this->thetaWorld); - this->go_turn_kalman(this->xWorld,this->yWorld,this->thetaWorld+PI/2); - - this->print_map_with_robot_position(); - pc.printf("\n\rX= %f",this->xWorld); - pc.printf("\n\rY= %f",this->yWorld); - pc.printf("\n\rtheta= %f",this->thetaWorld); - //left - this->go_to_point_with_angle_kalman(this->xWorld-sizeX,this->yWorld,this->thetaWorld); - this->go_turn_kalman(this->xWorld,this->yWorld,this->thetaWorld+PI/2); - - this->print_map_with_robot_position(); - pc.printf("\n\rX= %f",this->xWorld); - pc.printf("\n\rY= %f",this->yWorld); - pc.printf("\n\rtheta= %f",this->thetaWorld); - //down - this->go_to_point_with_angle_kalman(this->xWorld,this->yWorld-sizeY,this->thetaWorld); - this->go_turn_kalman(this->xWorld,this->yWorld,this->thetaWorld+PI/2); - - this->print_map_with_robot_position(); - pc.printf("\n\rX= %f",this->xWorld); - pc.printf("\n\rY= %f",this->yWorld); - pc.printf("\n\rtheta= %f",this->thetaWorld); - - } - */ + this->go_to_point_kalman(this->xWorld+sizeX,this->yWorld+sizeY); } //move of targetXWorld and targetYWorld ending in a targetAngleWorld @@ -817,6 +777,7 @@ do { leftMotor(1,50); rightMotor(0,50); + pc.printf("\r\n test lab 4: OdometriaKalmanFilter"); this->OdometriaKalmanFilter(); float distanceToTarget=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld); @@ -856,15 +817,13 @@ pc.printf("\r\nReached Target!"); } -//move of targetXWorld and targetYWorld ending in a targetAngleWorld -void MiniExplorerCoimbra::go_to_point_with_angle_kalman(float targetXWorld, float targetYWorld, float targetAngleWorld) { - //=====KINEMATICS=========================== +void MiniExplorerCoimbra::OdometriaKalmanFilter(){ + //=====KINEMATICS=========================== float R_cm; float L_cm; //fill R_cm and L_cm with how much is wheel has moved (custom robot.h) OdometriaKalman(&R_cm, &L_cm); - float encoderRightFailureRate=0.95; float encoderLeftFailureRate=1; @@ -878,13 +837,13 @@ float distanceMovedY=distanceMoved*sin(this->thetaWorld+angleMoved/2); //try with world coordinate system + myMatrix poseKplus1K(3,1); poseKplus1K.data[0][0]=this->xWorld+distanceMovedX; 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.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); - + + pc.printf("\r\n X=%f, Y=%f, theta=%f",poseKplus1K.get(0,0),poseKplus1K.get(1,0),poseKplus1K.get(2,0)); //=====ERROR_MODEL=========================== //FP Matrix @@ -1001,7 +960,7 @@ float frontCm = get_distance_front_sensor()/10; float rightCm = get_distance_right_sensor()/10; - pc.printf("\r\n1: Lcm=%f, FCm=%f, RCm=%f",leftCm,frontCm,rightCm); + pc.printf("\r\ndistance sonars Lcm=%f, FCm=%f, RCm=%f",leftCm,frontCm,rightCm); //if superior to sonar range, put the value to sonar max range + 1 if(leftCm > this->sonarLeft.maxRange) @@ -1100,262 +1059,7 @@ //for those who passed //compute composite innovation int nbPassed=leftPassed+frontPassed+rightPassed; - - - this->print_map_with_robot_position(); - pc.printf("\r\n E_LCm=%f, E_FCm=%f, E_RCm=%f",leftCmEstimated,frontCmEstimated,rightCmEstimated); - pc.printf("\r\n Lcm=%f, FCm=%f, RCm=%f",leftCm,frontCm,rightCm); - pc.printf("\r\n IL=%f, IF=%f, IR=%f",innovationLeft,innovationFront,innovationRight); - pc.printf("\r\n ICL=%f, ICF=%f, ICR=%f",innovationCovarianceLeft,innovationCovarianceFront,innovationCovarianceRight); - pc.printf("\r\n Gate score L=%f, F=%f, R=%f",gateScoreLeft,gateScoreFront,gateScoreRight); - pc.printf("\r\n nbPassed=%f",nbPassed); - wait(2); - -} - -void MiniExplorerCoimbra::OdometriaKalmanFilter(){ - //=====KINEMATICS=========================== - float R_cm; - float L_cm; - - //fill R_cm and L_cm with how much is wheel has moved (custom robot.h) - OdometriaKalman(&R_cm, &L_cm); - - float encoderRightFailureRate=0.95; - float encoderLeftFailureRate=1; - - R_cm=R_cm*encoderRightFailureRate; - L_cm=L_cm*encoderLeftFailureRate; - - float distanceMoved=(R_cm+L_cm)/2; - float angleMoved=(R_cm-L_cm)/this->distanceWheels; - - float distanceMovedX=distanceMoved*cos(this->thetaWorld+angleMoved/2); - float distanceMovedY=distanceMoved*sin(this->thetaWorld+angleMoved/2); - - //try with world coordinate system - myMatrix poseKplus1K(3,1); - poseKplus1K.data[0][0]=this->xWorld+distanceMovedX; - 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.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); - - //=====ERROR_MODEL=========================== - - //FP Matrix - myMatrix Fp(3,3); - Fp.data[0][0]=1; - Fp.data[1][1]=1; - Fp.data[2][2]=1; - Fp.data[0][2]=-1*distanceMoved*sin(this->thetaWorld+(angleMoved/2)); - Fp.data[1][2]=distanceMoved*cos(this->thetaWorld+(angleMoved/2)); - - myMatrix FpTranspose(3,3); - FpTranspose.fillWithTranspose(Fp); - - //Frl matrix - 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)); - Frl.data[1][0]=0.5*sin(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); - Frl.data[1][1]=0.5*sin(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); - Frl.data[2][0]=(1/this->distanceWheels); - Frl.data[2][1]=-(1/this->distanceWheels) ; - - myMatrix FrlTranspose(2,3); - FrlTranspose.fillWithTranspose(Frl); - - //error constants... - float kr=1; - float kl=1; - myMatrix covar(2,2); - covar.data[0][0]=kr*abs(R_cm); - covar.data[1][1]=kl*abs(L_cm); - - //uncertainty positionx, and theta at - //1,1,1 - myMatrix Q(3,3); - Q.data[0][0]=1; - Q.data[1][1]=2; - Q.data[2][2]=0.01; - - //new covariance= Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q - myMatrix covariancePositionEstimationKplus1K(3,3); - myMatrix temp1(3,3); - temp1.fillByMultiplication(Fp,this->covariancePositionEstimationK);//Fp*old covariance - myMatrix temp2(3,3); - temp2.fillByMultiplication(temp1,FpTranspose);//Fp*old covariance*FpTranspose - temp1.fillWithZeroes(); - myMatrix temp3(3,2); - temp3.fillByMultiplication(Frl,covar);//Frl*covar - temp1.fillByMultiplication(temp3,FrlTranspose);//Frl*covar*FrlTranspose - - covariancePositionEstimationKplus1K.addition(temp2);//Fp*old covariance*FpTranspose - covariancePositionEstimationKplus1K.addition(temp1);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose - covariancePositionEstimationKplus1K.addition(Q);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q - - //=====OBSERVATION===== - //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 xClosestCellLeft; - float yClosestCellLeft; - float xClosestCellFront; - float yClosestCellFront; - float xClosestCellRight; - float yClosestCellRight; - //get theorical distance to sonar - 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)>0.5){ - //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,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); - if((currDistance < this->sonarLeft.maxRange) && currDistance > -1){ - //check if distance is lower than previous, update lowest if so - if(currDistance < leftCmEstimated){ - leftCmEstimated= currDistance; - xClosestCellLeft=xWorldCell; - yClosestCellLeft=yWorldCell; - } - } - //check front - currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); - if((currDistance < this->sonarFront.maxRange) && currDistance > -1){ - //check if distance is lower than previous, update lowest if so - if(currDistance < frontCmEstimated){ - frontCmEstimated= currDistance; - xClosestCellFront=xWorldCell; - yClosestCellFront=yWorldCell; - } - } - //check right - currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); - if((currDistance < this->sonarRight.maxRange) && currDistance > -1){ - //check if distance is lower than previous, update lowest if so - if(currDistance < rightCmEstimated){ - rightCmEstimated= currDistance; - xClosestCellRight=xWorldCell; - yClosestCellRight=yWorldCell; - } - } - } - } - } - - //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; - - pc.printf("\r\n1: Lcm=%f, FCm=%f, RCm=%f",leftCm,frontCm,rightCm); - - //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; - - //======INNOVATION======== - //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 - myMatrix jacobianOfObservationLeft(1,3); - 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])); - - myMatrix jacobianOfObservationRightTranspose(3,1); - jacobianOfObservationRightTranspose.fillWithTranspose(jacobianOfObservationRight); - - myMatrix jacobianOfObservationFrontTranspose(3,1); - jacobianOfObservationFrontTranspose.fillWithTranspose(jacobianOfObservationFront); - - myMatrix jacobianOfObservationLeftTranspose(3,1); - jacobianOfObservationLeftTranspose.fillWithTranspose(jacobianOfObservationLeft); - //error constants 0,0,0 sonars perfect; must be found by experimenting; gives mean and standanrt deviation - //let's assume - //in centimeters - float R_front=4; - float R_left=4; - float R_right=4; - - //R-> 4 in diagonal - - //S for each sonar (concatenated covariance matrix of innovation) - //equ (12), innovationCovariance =JacobianObservation*covariancePositionEstimationKplus1K*JacobianObservationTranspose+R - myMatrix temp4(1,3); - temp4.fillByMultiplication(jacobianOfObservationFront,covariancePositionEstimationKplus1K); - myMatrix temp5(1,1); - temp5.fillByMultiplication(temp4,jacobianOfObservationFrontTranspose); - float innovationCovarianceFront=temp5.data[0][0]+R_front; - temp4.fillWithZeroes(); - temp5.fillWithZeroes(); - - - temp4.fillByMultiplication(jacobianOfObservationLeft,covariancePositionEstimationKplus1K); - temp5.fillByMultiplication(temp4,jacobianOfObservationLeftTranspose); - float innovationCovarianceLeft=temp5.data[0][0]+R_left; - temp4.fillWithZeroes(); - temp5.fillWithZeroes(); - - - temp4.fillByMultiplication(jacobianOfObservationRight,covariancePositionEstimationKplus1K); - temp5.fillByMultiplication(temp4,jacobianOfObservationRightTranspose); - float innovationCovarianceRight=temp5.data[0][0]+R_right; - - //check if it pass the validation gate - float gateScoreLeft=innovationLeft*innovationLeft/innovationCovarianceLeft; - float gateScoreFront=innovationFront*innovationFront/innovationCovarianceFront; - float gateScoreRight=innovationRight*innovationRight/innovationCovarianceRight; - int leftPassed=0; - int frontPassed=0; - int rightPassed=0; - - //5cm -> 25 - int errorsquare=25;//constant error - - if(gateScoreLeft <= errorsquare) - leftPassed=1; - if(gateScoreFront <= errorsquare) - frontPassed=10; - if(gateScoreRight <= errorsquare) - rightPassed=100; - //for those who passed - //compute composite innovation - int nbPassed=leftPassed+frontPassed+rightPassed; + pc.printf("\r\n nbPassed=%d, ",nbPassed); //compositeMeasurementJacobian myMatrix compositeMeasurementJacobian1x3(1,3); @@ -1398,7 +1102,7 @@ //new pose estimation myMatrix poseKplus1Kplus1(3,1); - poseKplus1Kplus1.fillByCopy(poseKplus1Kplus1); + poseKplus1Kplus1.fillByCopy(poseKplus1K); myMatrix tempPoseKplus1Kplus1(3,1); //covariancePositionEstimationKplus1Kplus1 @@ -1661,7 +1365,7 @@ this->yWorld=poseKplus1Kplus1.data[1][0]; this->thetaWorld=poseKplus1Kplus1.data[2][0]; - pc.printf("\r\nAfter Kalm X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld); + pc.printf("\r\nAfter Kalman X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld); //try with robot one /* X=xEstimatedKNext; @@ -1683,5 +1387,73 @@ //update odometrie X Y theta... } +void MiniExplorerCoimbra::go_to_point_kalman(float targetXWorld, float targetYWorld) { + + float angleError; //angle error + float d; //distance from target + float angularLeft, angularRight, linear, angular; + int speed=300; + do { + //Updating X,Y and theta with the odometry values + this->OdometriaKalmanFilter(); + //Computing angle error and distance towards the target value + angleError = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld; + if(angleError>PI) angleError=-(angleError-PI); + else if(angleError<-PI) angleError=-(angleError+PI); + //pc.printf("\n\r error=%f",angleError); + + d=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld); + //pc.printf("\n\r dist=%f/n", d); + + //Computing linear and angular velocities + linear=k_linear*d; + angular=k_angular*angleError; + angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels; + angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels; + + //Normalize speed for motors + if(angularLeft>angularRight) { + angularRight=speed*angularRight/angularLeft; + angularLeft=speed; + } else { + angularLeft=speed*angularLeft/angularRight; + angularRight=speed; + } + + //pc.printf("\n\r X=%f", this->xWorld); + //pc.printf("\n\r Y=%f", this->yWorld); + //pc.printf("\n\r theta=%f", this->thetaWorld); + + //Updating motor velocities + if(angularLeft>0){ + leftMotor(1,angularLeft); + } + else{ + leftMotor(0,-angularLeft); + } + + if(angularRight>0){ + rightMotor(1,angularRight); + } + else{ + rightMotor(0,-angularRight); + } + + //wait(0.5); + } while(d>5); + + //Stop at the end + leftMotor(1,0); + rightMotor(1,0); +} + +void MiniExplorerCoimbra::printMatrix(myMatrix mat1){ + for(int i = 0; i < mat1.nbRow; ++i) { + for(int j = 0; j < mat1.nbColumn; ++j) { + pc.printf("\r%f",mat1.data[i][j]); + } + pc.printf("\n"); + } +} \ No newline at end of file