Revision 14:696187e74411, committed 2017-07-12

Comitter:

Ludwigfr

Date:

Wed Jul 12 18:08:07 2017 +0000

Parent:

13:b0ddd71e8f08

Commit message:

lol

Changed in this revision

--- a/ISR_Mini-explorer.lib	Wed Jul 12 09:07:31 2017 +0000
+++ b/ISR_Mini-explorer.lib	Wed Jul 12 18:08:07 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/Ludwigfr/code/ISR_Mini-explorer/#92ca9a6a1d4e
+https://developer.mbed.org/users/Ludwigfr/code/ISR_Mini-explorer/#b996cbe5dabd

--- a/MiniExplorerCoimbra.cpp	Wed Jul 12 09:07:31 2017 +0000
+++ b/MiniExplorerCoimbra.cpp	Wed Jul 12 18:08:07 2017 +0000
@@ -12,7 +12,7 @@
 
     this->setXYThetaAndXYThetaWorld(defaultXWorld,defaultYWorld,defaultThetaWorld);
     this->radiusWheels=3.25;
-    this->distanceWheels=7.2; 
+    this->distanceWheels=7.18; 
     
     //go to point
     this->k_linear=10;
@@ -30,11 +30,16 @@
     
     this->speed=300;
 
-    this->rangeForce=50;
-    this->attractionConstantForce=1;
+    this->rangeForce=75;
+    this->attractionConstantForce=4;
     //-90 ok with attraction impacted by distance
-    this->repulsionConstantForce=-90;
+    this->repulsionConstantForce=-200;
     
+    this->D_cm=0;
+    this->L_cm=0;
+    
+    this->ticks2d_Special=0;
+    this->ticks2e_Special=0;
 } 
 
 void MiniExplorerCoimbra::setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld){
@@ -47,6 +52,13 @@
 		theta=PI/2+PI-defaultThetaWorld;
 	else
 		theta=defaultThetaWorld-PI/2;
+	this->myX_r=X;
+    this->myY_r=Y;
+    this->mytheta_r=theta;
+    
+    this->myX_r_correct=X;
+    this->myY_r_correct=Y;
+    this->mytheta_r_correct=theta;
 }
 
 void MiniExplorerCoimbra::myOdometria(){
@@ -762,236 +774,269 @@
 void MiniExplorerCoimbra::test_procedure_lab_4(float sizeX, float sizeY){
 	
 	this->map.fill_map_with_kalman_knowledge();
-	this->go_to_point_kalman(this->xWorld+sizeX,this->yWorld+sizeY);
+	this->go_to_point_kalman2(this->xWorld+sizeX,this->yWorld+sizeY);
+}
+
+void MiniExplorerCoimbra::myOdometriaBis(){
+	float R,L;
+	this->testOdometria(&R,&L);
+
+	this->xWorld=-Y;
+	this->yWorld=X;
+	if(theta >PI/2)
+		this->thetaWorld=-PI+(theta-PI/2);
+	else
+		this->thetaWorld=theta+PI/2;
 }
 
-//move of targetXWorld and targetYWorld ending in a targetAngleWorld
-void MiniExplorerCoimbra::go_turn_kalman(float targetXWorld, float targetYWorld, float targetAngleWorld) {
-    //make sure the target is correct
-    if(targetAngleWorld > PI)
-    	targetAngleWorld=-2*PI+targetAngleWorld;
-   	if(targetAngleWorld < -PI)
-   		targetAngleWorld=2*PI+targetAngleWorld;
-   		
-	float distanceToTarget=100;
-    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);
-        //pc.printf("\n\rdist to target= %f",distanceToTarget);
+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();
+		pc.printf("\n\r X=%f", this->xWorld);
+		pc.printf("\n\r Y=%f", this->yWorld);
+    	pc.printf("\n\r theta=%f", this->thetaWorld);
+        //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;
         
-    } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));
+
+        //Normalize speed for motors
+        if(angularLeft>angularRight) {
+            angularRight=speed*angularRight/angularLeft;
+            angularLeft=speed;
+        } else {
+            angularLeft=speed*angularLeft/angularRight;
+            angularRight=speed;
+        }
 
+        //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>1);
+	pc.printf("\r\n REACHED");
     //Stop at the end
     leftMotor(1,0);
     rightMotor(1,0);
-    pc.printf("\r\nReached Target!");
 }
 
-//move of targetXWorld and targetYWorld ending in a targetAngleWorld
-void MiniExplorerCoimbra::go_straight_kalman(float targetXWorld, float targetYWorld, float targetAngleWorld) {
-    //make sure the target is correct
-    if(targetAngleWorld > PI)
-    	targetAngleWorld=-2*PI+targetAngleWorld;
-   	if(targetAngleWorld < -PI)
-   		targetAngleWorld=2*PI+targetAngleWorld;
-   		
-	float distanceToTarget=100;;
-	
-    do {
-        leftMotor(1,400);
-	    rightMotor(1,400);
-	    this->OdometriaKalmanFilter();
-	    
-	    float distanceToTarget=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
-        pc.printf("\n\rdist to target= %f",distanceToTarget);
+void MiniExplorerCoimbra::go_to_point_kalman2(float targetXWorld, float targetYWorld) {
+    //float d; //distance from target
+    float angularLeft, angularRight;
+	float correctX;
+    float correctY;
+    float correctTheta;
+    float xAtEnd=targetXWorld;
+    do {        
+        //Updating X,Y and theta with the odometry values
+        this->OdometriaKalmanFilter();
+    	
+        correctX=-this->myY_r_correct;
+	    correctY=this->myX_r_correct;
+		if(this->mytheta_r_correct >PI/2)
+			correctTheta=-PI+(this->mytheta_r_correct-PI/2);
+		else
+			correctTheta=this->mytheta_r_correct+PI/2;
+			
+		pc.printf("\n\rFalse: X=%f,Y=%f,theta=%f", this->xWorld,this->yWorld,this->thetaWorld);
+		pc.printf("\n\rTrue : X=%f,Y=%f,theta=%f", correctX,correctY,correctTheta);	
+		//pc.printf("\n\rFalse Robot: X=%f,Y=%f,theta=%f", this->myX_r,this->myY_r,this->mytheta_r);
+		//pc.printf("\n\rTrue Robot: X=%f,Y=%f,theta=%f", this->myX_r_correct,this->myY_r_correct,this->mytheta_r_correct);
+        //d=this->dist(correctX, correctY, targetXWorld, targetYWorld);
+		//pc.printf("\n\r dist=%f/n", d);
+        angularLeft=200;
+        angularRight=200;
+        leftMotor(1,angularLeft);
+        rightMotor(1,angularRight);
         
-    } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));
-
+    } while((xAtEnd-correctX)>1);
     //Stop at the end
     leftMotor(1,0);
     rightMotor(1,0);
-    pc.printf("\r\nReached Target!");
+	pc.printf("\r\n REACHED");
+    
+    while(1){
+    	pc.printf("\n\rFalse: X=%f,Y=%f,theta=%f", this->xWorld,this->yWorld,this->thetaWorld);
+		pc.printf("\n\rTrue : X=%f,Y=%f,theta=%f", correctX,correctY,correctTheta);
+		//pc.printf("\n\rFalse Robot: X=%f,Y=%f,theta=%f", this->myX_r,this->myY_r,this->mytheta_r);
+		//pc.printf("\n\rTrue Robot: X=%f,Y=%f,theta=%f", this->myX_r_correct,this->myY_r_correct,this->mytheta_r_correct);
+    }
+}
+
+void MiniExplorerCoimbra::testOdometria(float *D_Special,float *L_Special){
+	float encoderRightFailureRate_Special=1;
+	float encoderLeftFailureRate_Special=0.95;
+	
+	long int ticks1d_Special=R_encoder();
+    long int ticks1e_Special=L_encoder();
+
+    long int D_ticks_Special=ticks1d_Special - this->ticks2d_Special;
+    long int E_ticks_Special=ticks1e_Special - this->ticks2e_Special;
+
+    this->ticks2d_Special=ticks1d_Special;
+    this->ticks2e_Special=ticks1e_Special;
+	
+    *D_Special= (float)D_ticks_Special*((3.25f*3.1415f)/4096);
+    *L_Special= (float)E_ticks_Special*((3.25f*3.1415f)/4096);
+    
+    float CM_O=(*D_Special + *L_Special)/2;
+
+    //pc.printf("\r\nCM:%f",CM);
+    this->mytheta_r_correct +=(*D_Special - *L_Special)/7.18;
+ 
+ 	//modify D, L
+    float temp_Special=*D_Special;
+    *D_Special=temp_Special*encoderRightFailureRate_Special;
+	temp_Special=*L_Special;
+	*L_Special=temp_Special*encoderLeftFailureRate_Special;
+	
+    float CM_Special=(*D_Special + *L_Special)/2;
+
+    this->mytheta_r +=(*D_Special -*L_Special)/7.18;
+
+    this->mytheta_r = atan2(sin(this->mytheta_r), cos(this->mytheta_r));
+    this->myX_r += CM_Special*cos(this->mytheta_r);
+    this->myY_r += CM_Special*sin(this->mytheta_r);
+    
+    this->mytheta_r_correct = atan2(sin(this->mytheta_r_correct), cos(this->mytheta_r_correct));
+    this->myX_r_correct += CM_O*cos(this->mytheta_r_correct);
+    this->myY_r_correct += CM_O*sin(this->mytheta_r_correct);
 }
 
 void MiniExplorerCoimbra::OdometriaKalmanFilter(){
-    float encoderRightFailureRate=1;
-	float encoderLeftFailureRate=1;
-	int skip=1;//1==skip Kalman correction, just use KINEMATICS
+   	float L_CM=0;
+	float R_CM=0;
+	int skip=0;//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];
-
-    i2c1.start();
-    i2c1.write(0x6C);
-    i2c1.write(0x0C);
-    i2c1.read(0x6D,data_r_2,4,0);
-    i2c1.stop();
-
-    short int val1=data_r_2[0];
-    short int val2=data_r_2[1];
-    val1=(val1&0xf)*256;
-    long int final=(val2+val1);
-    
-    ticks1d=final;
-    
-	long int ticks1e=L_encoder();
-	
-	char data_r_2_2[5];
-
-    i2c.start();
-    i2c.write(0x6C);
-    i2c.write(0x0C);
-    i2c.read(0x6D,data_r_2_2,4,0);
-    i2c.stop();
-
-    val1=data_r_2_2[0];
-    val2=data_r_2_2[1];
-    val1=(val1&0xf)*256;
-    final=(val2+val1);
-
-    ticks1e= final;
-    
-	pc.printf("\r\nEncoders ticks1d:%d,ticks1e:%d",ticks1d,ticks1e);	
-
-    long int R_ticks=ticks1d - ticks2d;
-    long int L_ticks=ticks1e - ticks2e;
-    
-	pc.printf("\r\nEncoders R_ticks:%d,L_ticks:%d",R_ticks,L_ticks);
-	//robot.h
-    ticks2d=ticks1d;
-    ticks2e=ticks1e;
-
-	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();
-    
-    R_cmK= encoderRightFailureRate*R_cmK;
-    L_cmK= encoderLeftFailureRate*L_cmK;
-	//float R_cmK=D_cm*encoderRightFailureRate;
-	//float L_cmK=L_cm*encoderLeftFailureRate;
-	
-	pc.printf("\r\nEncoders R_cm:%f,L_cm:%f",R_cmK,L_cmK);
-
-	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 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));
+	this->testOdometria(&R_CM,&L_CM);
+	//pc.printf("\n\rK3: D_cm:%f,L_cm:%f", (float)this->D_cm,(float)this->L_cm);
+    //pc.printf("\n\rK3: X_r=%f", X);
+	//pc.printf("\n\rK3: Y_r=%f", Y);
+    //pc.printf("\n\rK3: theta_r=%f", theta);
     
-    //currTheta to world coordinates
-    float newAngleThetaWorld;
-    if(currTheta >PI/2)
-		newAngleThetaWorld=-PI+(currTheta-PI/2);
+	float newxWorld=-this->myY_r;
+	float newyWorld=this->myX_r;
+	float newthetaWorld;
+	if(this->mytheta_r >PI/2)
+		newthetaWorld=-PI+(this->mytheta_r-PI/2);
 	else
-		newAngleThetaWorld=currTheta+PI/2;
-	//compute difference with cur angle
-	angleMoved = newAngleThetaWorld-this->thetaWorld;
-	if(angleMoved>PI) 
-		angleMoved=-(angleMoved-PI);
-    else 
-    	if(angleMoved<-PI) 
-    		angleMoved=-(angleMoved+PI);
-
-    pc.printf("\r\ndistanceMoved:%f, angleMoved:%f, totalAngle:%f",distanceMoved,angleMoved,newAngleThetaWorld);
-	//here testing with robot coordinates
-    float distanceMovedX=distanceMoved*cos(this->thetaWorld+(R_cmK-L_cmK)/(2*this->distanceWheels));
-    float distanceMovedY=distanceMoved*sin(this->thetaWorld+(R_cmK-L_cmK)/(2*this->distanceWheels));
-    pc.printf("\r\ndistanceMovedX:%f, distanceMovedY:%f",distanceMovedX,distanceMovedY);
-	
-	//try with world coordinate system
-	
+		newthetaWorld=this->mytheta_r+PI/2;
+		
+    float distanceMoved=(R_CM+L_CM)/2;
+	float angleMovedR=(R_CM-L_CM)/this->distanceWheels;
+	float angleMoved=0;
+	if(angleMovedR >PI/2)
+		angleMoved=-PI+(angleMovedR-PI/2);
+	else
+		angleMoved=angleMovedR+PI/2;
+		
+	newxWorld=this->xWorld+distanceMoved*cos(this->thetaWorld+angleMoved/2);
+    newyWorld=this->yWorld+distanceMoved*sin(this->thetaWorld+angleMoved/2);
+    newthetaWorld=this->thetaWorld+angleMoved;
+        
 	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]=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===========================
-
-   	//FP Matrix slide LocalizationKALMAN                     
-    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);
+	poseKplus1K.data[0][0]=newxWorld;
+	poseKplus1K.data[1][0]=newyWorld;
+	poseKplus1K.data[2][0]=newthetaWorld;
 	
-    //Frl matrix slide LocalizationKALMAN
-    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_cmK);
-    covar.data[1][1]=kl*abs(L_cmK);
-
-	//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 slide LocalizationKALMAN
-	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
-    
+	pc.printf("\r\nR:%f,L:%f",R_CM,L_CM);
 	if(skip==1){
-		this->covariancePositionEstimationK.fillByCopy(covariancePositionEstimationKplus1K);
-		
-		//pc.printf("\r\nposeKplus1Kplus1 X=%f, Y=%f, theta=%f",poseKplus1Kplus1.data[0][0],poseKplus1Kplus1.data[1][0],poseKplus1Kplus1.data[2][0]);
 		//update pose 
 		this->xWorld=poseKplus1K.data[0][0];
 		this->yWorld=poseKplus1K.data[1][0];
 		this->thetaWorld=poseKplus1K.data[2][0];
+		//pc.printf("\r\nWithout Kalman X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld);
+	}else{
+		//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===========================
+	
+	   	//FP Matrix slide LocalizationKALMAN                     
+	    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)); 
 		
-		pc.printf("\r\nWithout Kalman X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld);
-	}else{
+		myMatrix FpTranspose(3,3);
+		FpTranspose.fillWithTranspose(Fp);
+		pc.printf("\r\nFp");;
+	    //Frl matrix slide LocalizationKALMAN
+	    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);
+	    pc.printf("\r\nFrl");
+	     //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);
+		pc.printf("\r\ncovar");
+		//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;
+	    pc.printf("\r\nQ");
+	    //new covariance= Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q slide LocalizationKALMAN
+		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
+		//pc.printf("\r\ncovariancePositionEstimationKplus1K  1");
+		temp1.fillWithZeroes();
+		pc.printf("\r\ncovariancePositionEstimationKplus1K  1.1");
+		myMatrix temp3(3,2);
+		temp3.fillByMultiplication(Frl,covar);//Frl*covar
+		pc.printf("\r\ncovariancePositionEstimationKplus1K  1.2");
+		temp1.fillByMultiplication(temp3,FrlTranspose);//Frl*covar*FrlTranspose
+		pc.printf("\r\ncovariancePositionEstimationKplus1K  2");
+		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
+    	pc.printf("\r\ncovariancePositionEstimationKplus1K");
 		//=====OBSERVATION=====
 	    //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
 	    
@@ -1519,13 +1564,13 @@
 		//update covariancePositionEstimationK =covariancePositionEstimationKplus1Kplus1
 		this->covariancePositionEstimationK.fillByCopy(covariancePositionEstimationKplus1Kplus1);
 		
-		//pc.printf("\r\nposeKplus1Kplus1 X=%f, Y=%f, theta=%f",poseKplus1Kplus1.data[0][0],poseKplus1Kplus1.data[1][0],poseKplus1Kplus1.data[2][0]);
+		////pc.printf("\r\nposeKplus1Kplus1 X=%f, Y=%f, theta=%f",poseKplus1Kplus1.data[0][0],poseKplus1Kplus1.data[1][0],poseKplus1Kplus1.data[2][0]);
 		//update pose 
 		this->xWorld=poseKplus1Kplus1.data[0][0];
 		this->yWorld=poseKplus1Kplus1.data[1][0];
 		this->thetaWorld=poseKplus1Kplus1.data[2][0];
 		
-		pc.printf("\r\nWith Kalman X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld);
+		//pc.printf("\r\nWith Kalman X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld);
 		//try with robot one
 		/*
 		X=xEstimatedKNext;
@@ -1548,70 +1593,6 @@
 	}
 }
 
-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();
-		//this->myOdometria();
-        //pc.printf("\r\nD_cm:%f",D_cm);
-    	//pc.printf("\r\nL_cm:%f",L_cm);
-        //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) {

--- a/MiniExplorerCoimbra.hpp	Wed Jul 12 09:07:31 2017 +0000
+++ b/MiniExplorerCoimbra.hpp	Wed Jul 12 18:08:07 2017 +0000
@@ -51,6 +51,19 @@
 	float repulsionConstantForce;
 	
 	myMatrix covariancePositionEstimationK;
+	float D_cm;
+	float L_cm;
+	
+	float myX_r;
+    float myY_r;
+    float mytheta_r;
+    
+    float myX_r_correct;
+    float myY_r_correct;
+    float mytheta_r_correct;
+    
+    long int ticks2d_Special;
+    long int ticks2e_Special;
 
 	MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap);
 	
@@ -86,18 +99,24 @@
 	
 	void go_to_point_kalman(float targetXWorld, float targetYWorld);
 	
-	void OdometriaKalmanFilter(); 
+	void go_to_point_kalman2(float targetXWorld, float targetYWorld);
 	
 	void test_prediction_sonar();
 	
+	void OdometriaKalmanFilter(); 
+	
 	void printMatrix(myMatrix mat1);
 	
-	private:
+	void myOdometriaBis();
+	
+	void testOdometria(float *D_Special,float *L_Special);
 	
 	void myOdometria();
 	
 	void setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld);
 	
+	private:
+	
 	float update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt);
 	
 	void update_sonar_values(float leftMm,float frontMm,float rightMm);

--- a/myMatrix.cpp	Wed Jul 12 09:07:31 2017 +0000
+++ b/myMatrix.cpp	Wed Jul 12 18:08:07 2017 +0000
@@ -18,9 +18,19 @@
     this->fillWithZeroes();
 }
 
+/*
+myMatrix::~myMatrix()
+{
+    //for (int i = 0; i < this->nbRow; i++) {
+    //    delete [] (this->data[i]);
+    //}
+    //delete [] (this->data);
+    
+}
+*/
 void myMatrix::fillWithZeroes(){
-    for(int i = 0; i < this->nbRow; ++i) {
-        for(int j = 0; j < this->nbColumn; ++j) {
+    for(int i = 0; i < mat1.nbRow; ++i) {
+        for(int j = 0; j < mat1.nbColumn; ++j) {
             this->data[i][j] = 0;
         }
     }

--- a/myMatrix.hpp	Wed Jul 12 09:07:31 2017 +0000
+++ b/myMatrix.hpp	Wed Jul 12 18:08:07 2017 +0000
@@ -10,7 +10,9 @@
         float** data;
 
         myMatrix(int inNbRow,int inNbColumn);
-
+        
+        //~myMatrix();
+        
         void set(int i,int j, float value);
 
         float get(int i,int j);