Yo Fleyt
test morning
Dependencies: ISR_Mini-explorer mbed
Fork of
roboticLab_withclass_3_July
by 
Georgios Tsamis
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 10:d0109d7cbe7c
- Parent:
- 9:1cc27f33d3e1
- Child:
- 11:b91fe0ed4fed
--- 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