Diff: MiniExplorerCoimbra.cpp

Revision:

13:b0ddd71e8f08

Parent:

12:1e80471c5c6c

Child:

14:696187e74411

--- 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);