test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 13:b0ddd71e8f08
- Parent:
- 12:1e80471c5c6c
- Child:
- 14:696187e74411
diff -r 1e80471c5c6c -r b0ddd71e8f08 MiniExplorerCoimbra.cpp --- a/MiniExplorerCoimbra.cpp Tue Jul 11 16:55:13 2017 +0000 +++ b/MiniExplorerCoimbra.cpp Wed Jul 12 09:07:31 2017 +0000 @@ -818,8 +818,14 @@ } void MiniExplorerCoimbra::OdometriaKalmanFilter(){ + float encoderRightFailureRate=1; + float encoderLeftFailureRate=1; + int skip=1;//1==skip Kalman correction, just use KINEMATICS //=====KINEMATICS=========================== - + //we have trouble reading the encoders, in many case we only get 0 + float R_cmK; + float L_cmK; + /* long int ticks1d=R_encoder(); char data_r_2[5]; @@ -863,15 +869,14 @@ //robot.h ticks2d=ticks1d; ticks2e=ticks1e; - - //fill R_cm and L_cm with how much is wheel has moved (custom robot.h) - //OdometriaKalman(&R_cm, &L_cm); + + R_cmK= (float)R_ticks*((3.25*3.1415)/4096); + L_cmK= (float)L_ticks*((3.25*3.1415)/4096); + */ + //fill R_cmK and L_cmK with how much is wheel has moved (custom robot.h) + OdometriaKalman(&R_cmK, &L_cmK); //Odometria(); - float encoderRightFailureRate=1; - float encoderLeftFailureRate=1; - - float R_cmK= (float)R_ticks*((3.25*3.1415)/4096); - float L_cmK= (float)L_ticks*((3.25*3.1415)/4096); + R_cmK= encoderRightFailureRate*R_cmK; L_cmK= encoderLeftFailureRate*L_cmK; //float R_cmK=D_cm*encoderRightFailureRate; @@ -881,18 +886,19 @@ float distanceMoved=(R_cmK+L_cmK)/2; float angleMoved=(R_cmK-L_cmK)/this->distanceWheels; - angleMoved = atan2(sin(angleMoved), cos(angleMoved)); - //this->thetaWorld to theta + //this->thetaWorld to theta robot float currTheta; if(this->thetaWorld < -PI/2) currTheta=PI/2+PI-this->thetaWorld; else currTheta=this->thetaWorld-PI/2; + currTheta=currTheta+angleMoved; currTheta = atan2(sin(currTheta), cos(currTheta)); - //newAngleThetaRobot to world coordinates + + //currTheta to world coordinates float newAngleThetaWorld; if(currTheta >PI/2) newAngleThetaWorld=-PI+(currTheta-PI/2); @@ -917,8 +923,9 @@ 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; - + //poseKplus1K.data[2][0]=this->thetaWorld+angleMoved; + poseKplus1K.data[2][0]=newAngleThetaWorld; + //note if the encoders are correct it should work pc.printf("\r\nWithout correction: X=%f, Y=%f, theta=%f",poseKplus1K.get(0,0),poseKplus1K.get(1,0),poseKplus1K.get(2,0)); //=====ERROR_MODEL=========================== @@ -974,7 +981,6 @@ covariancePositionEstimationKplus1K.addition(temp1);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose covariancePositionEstimationKplus1K.addition(Q);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q - int skip=1; if(skip==1){ this->covariancePositionEstimationK.fillByCopy(covariancePositionEstimationKplus1K); @@ -1069,17 +1075,30 @@ myMatrix jacobianOfObservationFront(1,3); //(5) + //note: for cos/sin having abs(angles) > PI is not an issue float xSonarLeft=poseKplus1K.data[0][0]+this->sonarLeft.distanceX*cos(poseKplus1K.data[0][2])-this->sonarLeft.distanceY*sin(poseKplus1K.data[0][2]); float ySonarLeft=poseKplus1K.data[1][0]+this->sonarLeft.distanceX*sin(poseKplus1K.data[0][2])-this->sonarLeft.distanceY*cos(poseKplus1K.data[0][2]); float angleSonarLeft=poseKplus1K.data[0][2]+this->sonarLeft.angleFromCenter; + if(angleSonarLeft > PI) + angleSonarLeft=angleSonarLeft-2*PI; + if(angleSonarLeft < -PI) + angleSonarLeft=angleSonarLeft+2*PI; float xSonarFront=poseKplus1K.data[0][0]+this->sonarFront.distanceX*cos(poseKplus1K.data[0][2])-this->sonarFront.distanceY*sin(poseKplus1K.data[0][2]); float ySonarFront=poseKplus1K.data[1][0]+this->sonarFront.distanceX*sin(poseKplus1K.data[0][2])-this->sonarFront.distanceY*cos(poseKplus1K.data[0][2]); float angleSonarFront=poseKplus1K.data[0][2]+this->sonarFront.angleFromCenter; + if(angleSonarFront > PI) + angleSonarFront=angleSonarFront-2*PI; + if(angleSonarFront < -PI) + angleSonarFront=angleSonarFront+2*PI; float xSonarRight=poseKplus1K.data[0][0]+this->sonarRight.distanceX*cos(poseKplus1K.data[0][2])-this->sonarRight.distanceY*sin(poseKplus1K.data[0][2]); float ySonarRight=poseKplus1K.data[1][0]+this->sonarRight.distanceX*sin(poseKplus1K.data[0][2])-this->sonarRight.distanceY*cos(poseKplus1K.data[0][2]); float angleSonarRight=poseKplus1K.data[0][2]+this->sonarRight.angleFromCenter; + if(angleSonarRight > PI) + angleSonarRight=angleSonarRight-2*PI; + if(angleSonarRight < -PI) + angleSonarRight=angleSonarRight+2*PI; //note: last line divided by d, [0][2] all angles are theta^ teacher's advice, error in paper //left @@ -1106,8 +1125,7 @@ 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 + //error constants 0,0,0 sonars perfect; must be found by experimenting; gives mean and standart deviation //in centimeters float R_front=4; float R_left=4; @@ -1115,7 +1133,7 @@ //R-> 4 in diagonal - //S for each sonar (concatenated covariance matrix of innovation) + //for each sonar (concatenated covariance matrix of innovation) //equ (12), innovationCovariance =JacobianObservation*covariancePositionEstimationKplus1K*JacobianObservationTranspose+R myMatrix temp4(1,3); temp4.fillByMultiplication(jacobianOfObservationFront,covariancePositionEstimationKplus1K);