Yo Fleyt
test morning
Dependencies: ISR_Mini-explorer mbed
Fork of
roboticLab_withclass_3_July
by 
Georgios Tsamis
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 6:0e8db3a23486
- Parent:
- 5:19f24c363418
- Child:
- 8:072a76960e27
--- 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;