test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 6:0e8db3a23486
- Parent:
- 5:19f24c363418
- Child:
- 8:072a76960e27
diff -r 19f24c363418 -r 0e8db3a23486 MiniExplorerCoimbra.cpp --- a/MiniExplorerCoimbra.cpp Thu Jul 06 16:51:40 2017 +0000 +++ b/MiniExplorerCoimbra.cpp Fri Jul 07 11:49:30 2017 +0000 @@ -816,7 +816,7 @@ do { leftMotor(1,50); rightMotor(0,50); - this->OdometriaKalmanFilter(1,1); + this->OdometriaKalmanFilter(); float distanceToTarget=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld); //pc.printf("\n\rdist to target= %f",distanceToTarget); @@ -842,7 +842,7 @@ do { leftMotor(1,400); rightMotor(1,400); - this->OdometriaKalmanFilter(1,1); + this->OdometriaKalmanFilter(); float distanceToTarget=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld); pc.printf("\n\rdist to target= %f",distanceToTarget); @@ -873,7 +873,7 @@ t.start(); //Updating X,Y and theta with the odometry values - this->OdometriaKalmanFilter(1,1); + this->OdometriaKalmanFilter(); //Updating motor velocities distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt); @@ -883,7 +883,7 @@ t.stop(); pc.printf("\n\rdist to target= %f",distanceToTarget); - } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1)); + } while(distanceToTarget>10 || (abs(targetAngleWorld-this->thetaWorld)>0.1)); //Stop at the end leftMotor(1,0); @@ -891,7 +891,9 @@ pc.printf("\r\nReached Target!"); } -void MiniExplorerCoimbra::OdometriaKalmanFilter(float encoderRightFailureRate,float encoderLeftFailureRate){ +void MiniExplorerCoimbra::test_prediction_sonar(){ + this->map.fill_map_with_kalman_knowledge(); + //=====KINEMATICS=========================== float R_cm; float L_cm; @@ -899,8 +901,8 @@ //fill R_cm and L_cm with how much is wheel has moved (custom robot.h) OdometriaKalman(&R_cm, &L_cm); - encoderRightFailureRate=0.95; - encoderLeftFailureRate=1; + float encoderRightFailureRate=0.95; + float encoderLeftFailureRate=1; R_cm=R_cm*encoderRightFailureRate; L_cm=L_cm*encoderLeftFailureRate; @@ -917,6 +919,226 @@ float yEstimatedK=this->yWorld+distanceMovedY; float thetaWorldEstimatedK = this->thetaWorld+angleMoved; + pc.printf("\r\n X=%f, Y=%f, theta=%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + + //try with robot coordinate system + /* + float xEstimatedK=X; + float yEstimatedK=Y; + float thetaWorldEstimatedK = theta; + */ + //=====ERROR_MODEL=========================== + + //FP Matrix + float Fp[3][3]={{1,0,0},{0,1,0},{0,0,1}}; + + Fp[0][2]=-1*distanceMoved*sin(this->thetaWorld+(angleMoved/2)); + Fp[1][2]=distanceMoved*cos(this->thetaWorld+(angleMoved/2)); + + //Frl matrix + float Frl[3][2]={{0,0},{0,0},{(1/this->distanceWheels),-(1/this->distanceWheels)}}; + + Frl[0][0]=0.5*cos(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); + Frl[0][1]=0.5*cos(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); + Frl[1][0]=0.5*sin(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); + Frl[1][1]=0.5*sin(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); + + //error constants... + float kr=1; + float kl=1; + float covar[2][2]={{kr*abs(R_cm), 0}, {0, kl*abs(L_cm)}}; + + //uncertainty positionx, and theta at + //1,1,1 + float Q[3][3]={{1,0,0}, {0,2,0}, {0,0,0.01}}; + + covariancePositionEstimationK[0][0]=covar[0][0]*pow(Frl[0][0],2)+covar[1][1]*pow(Frl[0][1],2)+covariancePositionEstimationK[0][0]+Q[0][0]+covariancePositionEstimationK[2][0]*Fp[0][2]+Fp[0][2]*(covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2]); + covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1]+covariancePositionEstimationK[2][1]*Fp[0][2]+Fp[1][2]*(covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2])+covar[0][0]*Frl[0][0]*Frl[1][0]+covar[1][1]*Frl[0][1]; + covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2]+covar[0][0]*Frl[0][0]*Frl[2][0]+covar[1][1]*Frl[0][1]*Frl[2][1]; + covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0]+covariancePositionEstimationK[2][0]*Fp[1][2]+Fp[0][2]*(covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2])+covar[0][0]*Frl[0][0]*Frl[1][0]+covar[1][1]*Frl[0][1]*Frl[1][1]; + covariancePositionEstimationK[1][1]=covar[0][0]*pow(Frl[1][0],2)+covar[1][1]*pow(Frl[1][1],2)+covariancePositionEstimationK[1][1]+Q[1][1]+covariancePositionEstimationK[2][1]*Fp[1][2]+Fp[1][2]*(covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2]); + covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2]+covar[0][0]*Frl[1][0]*Frl[2][0]+covar[1][1]*Frl[1][1]*Frl[2][1]; + covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0]+covariancePositionEstimationK[2][2]*Fp[0][2]+covar[0][0]*Frl[0][0]*Frl[2][0]+covar[1][1]*Frl[0][1]*Frl[2][1]; + covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1]+covariancePositionEstimationK[2][2]*Fp[1][2]+covar[0][0]*Frl[1][0]*Frl[2][0]+covar[1][1]*Frl[1][1]*Frl[2][1]; + covariancePositionEstimationK[2][2]=covar[0][0]*pow(Frl[2][1],2)+covar[1][1]*pow(Frl[2][1],2)+covariancePositionEstimationK[2][2]+Q[2][2]; + + + //=====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,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + 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,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + 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,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + 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 + float jacobianOfObservationLeft[1][3]; + float jacobianOfObservationRight[1][3]; + float jacobianOfObservationFront[1][3]; + float xSonarLeft=xEstimatedK+this->sonarLeft.distanceX; + float ySonarLeft=yEstimatedK+this->sonarLeft.distanceY; + //left + jacobianOfObservationLeft[0][0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated; + jacobianOfObservationLeft[0][1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated; + jacobianOfObservationLeft[0][2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaWorldEstimatedK)+ySonarLeft*cos(thetaWorldEstimatedK))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaWorldEstimatedK)+ySonarLeft*sin(thetaWorldEstimatedK)); + //front + float xSonarFront=xEstimatedK+this->sonarFront.distanceX; + float ySonarFront=yEstimatedK+this->sonarFront.distanceY; + jacobianOfObservationFront[0][0]=(xSonarFront-xClosestCellFront)/frontCmEstimated; + jacobianOfObservationFront[0][1]=(ySonarFront-yClosestCellFront)/frontCmEstimated; + jacobianOfObservationFront[0][2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaWorldEstimatedK)+ySonarFront*cos(thetaWorldEstimatedK))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaWorldEstimatedK)+ySonarFront*sin(thetaWorldEstimatedK)); + //right + float xSonarRight=xEstimatedK+this->sonarRight.distanceX; + float ySonarRight=yEstimatedK+this->sonarRight.distanceY; + jacobianOfObservationRight[0][0]=(xSonarRight-xClosestCellRight)/rightCmEstimated; + jacobianOfObservationRight[0][1]=(ySonarRight-yClosestCellRight)/rightCmEstimated; + jacobianOfObservationRight[0][2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaWorldEstimatedK)+ySonarRight*cos(thetaWorldEstimatedK))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaWorldEstimatedK)+ySonarRight*sin(thetaWorldEstimatedK)); + + //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) + float innovationCovarianceFront=R_front+ jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); + float innovationCovarianceLeft=R_left+ jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); + float innovationCovarianceRight=R_right+ jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); + + //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; + + + this->print_map_with_robot_position(); + pc.printf("\r\n E_LCm=%f, E_FCm=%f, E_RCm=%f",frontCmEstimated,leftCmEstimated,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 + + float xEstimatedK=this->xWorld+distanceMovedX; + float yEstimatedK=this->yWorld+distanceMovedY; + float thetaWorldEstimatedK = this->thetaWorld+angleMoved; + + pc.printf("\r\n X=%f, Y=%f, theta=%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + //try with robot coordinate system /* float xEstimatedK=X; @@ -983,7 +1205,7 @@ yWorldCell=this->map.cell_height_coordinate_to_world(j); //check left currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); - if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1){ + if((currDistance < this->sonarLeft.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < leftCmEstimated){ leftCmEstimated= currDistance; @@ -993,7 +1215,7 @@ } //check front currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); - if((currDistance < this->sonarFront.maxRange) && currDistance!=-1){ + if((currDistance < this->sonarFront.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < frontCmEstimated){ frontCmEstimated= currDistance; @@ -1003,7 +1225,7 @@ } //check right currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); - if((currDistance < this->sonarRight.maxRange) && currDistance!=-1){ + if((currDistance < this->sonarRight.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < rightCmEstimated){ rightCmEstimated= currDistance; @@ -1077,6 +1299,7 @@ int frontPassed=0; int rightPassed=0; + pc.printf("\r\n Gate scre L=%f, F=%f, R=%f",gateScoreLeft,gateScoreFront,gateScoreRight); //5cm -> 25 int errorsquare=25;//constant error @@ -1311,7 +1534,7 @@ this->yWorld=yEstimatedKNext; this->thetaWorld=thetaWorldEstimatedKNext; - + pc.printf("\r\nAfter Kalm X=%f, Y=%f, theta=%f",xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext); //try with robot one /* X=xEstimatedKNext;