Diff: main.cpp

Revision:

30:33e970ba1fe5

Parent:

28:0a659a910771

Child:

31:e60cd1c984f4

diff -r c990a5b0515f -r 33e970ba1fe5 main.cpp
--- a/main.cpp	Mon Jan 26 22:40:37 2015 +0000
+++ b/main.cpp	Sun Feb 01 19:29:14 2015 +0000
@@ -110,9 +110,9 @@
     
     /*desired State : (x y theta phiright phileft)*/
     Mat<double> dX((double)0, nbrstate, 1);
-    dX.set( (double)100, 1,1);
+    dX.set( (double)10, 1,1);
     dX.set( (double)0, 2,1);
-    dX.set( (double)0, 3,1);
+    dX.set( (double)PI/2, 3,1);
     dX.set( (double)0, 4,1);
     dX.set( (double)0, 5,1);    
     
@@ -143,7 +143,7 @@
     /*----------------------------------------------------------------------------------------------*/
     
     
-    while(abs(instance.getX().get(1,1)-100) > 10)
+    while(abs(instance.getX().get(1,1)-50) > 10)
     {       
         t.start(); 
         //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);                
@@ -156,17 +156,23 @@
         instance.measurement_Callback( z, dX, true );
          
         instance.state_Callback();
-                
+        //instance.setX(z);
+        
+        //instance.computeCommand(dX, (double)dt, -2);                    
+        instance.computeCommand(dX, (double)t.read(), -2);                    
         double phi_r = instance.getCommand().get(1,1);
         double phi_l = instance.getCommand().get(2,1);
         
         double phi_max = 1.0;                    
-        
-        instance.computeCommand(dX, (double)dt, -2);            
+                
         //pcs.printf("command : \n phi_r = %f \n phi_l = %f \n", ((double)phi_r/phi_max), ((double)phi_l/phi_max));
-        (instance.getCommand()).afficher();    
-        (instance.getX()).afficher();
-        (instance.getSigma()).afficher();
+        //(instance.getCommand()).afficher();    
+        pcs.printf("State : \n\r");
+        (instance.getX()).afficherM();
+        //pcs.printf("Sigma : \n\r");
+        //(instance.getSigma()).afficher();
+        pcs.printf("Kalman Gain : \n\r");
+        (instance.getKGain()).afficherM();
         
         
         if(phi_r >= 0)
@@ -179,26 +185,33 @@
         else
             dir2.write(1);
             
-        if(abs(phi_r/phi_max) < 1.0)
+        if(abs(phi_r/phi_max) <= 1.0)
         {
             //pcs.printf( "VALUE PWM 1 : %f \n", (float)abs((double)phi_r/phi_max)) ;
             setPWM(&pw1, (float)abs((double)phi_r/phi_max));
         }
         else
+        {
             pcs.printf("P1...");
+            setPWM(&pw1, (float)abs((double)phi_max/phi_max));
+        }
             
-        if(abs(phi_l/phi_max) < 1.0)
+        if(abs(phi_l/phi_max) <= 1.0)
         {
             //pcs.printf( "VALUE PWM 2 : %f \n", (float)abs((double)phi_l/phi_max)) ;
             setPWM(&pw2,(float)abs((double)phi_l/phi_max));
         }
         else
+        {
             pcs.printf("P2...");
+            setPWM(&pw2, (float)abs((double)phi_max/phi_max));
+        }
             
         //pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n");
         t.stop();
         pcs.printf("%f s \n\r", t.read());
         t.reset();
+        t.start();
                 
     }
         
@@ -219,7 +232,13 @@
 {
     z->set( (double)/*conversionUnitée mm */odometry->getX(), 1,1);
     z->set( (double)/*conversionUnitée mm*/odometry->getY(), 2,1);
-    z->set( (double)/*conversionUnitée rad*/odometry->getTheta(), 3,1);    
+    z->set( (double)/*conversionUnitée rad*/odometry->getTheta(), 3,1);   
+    double vx = (double)odometry->getVx();
+    double vy = (double)odometry->getVy();
+    z->set( sqrt(vx*vx+vy*vy),4,1);
+    z->set( (double)odometry->getW(),5,1); 
+    
+    z->afficherM();
 }
 
 Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt)
@@ -268,6 +287,7 @@
 
 Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
 {    
+    /*
     double angle = state.get(3,1);
     if( angle < -PI)
     {
@@ -279,31 +299,32 @@
     }
     
     state.set( atan21(sin(angle), cos(angle)), 3,1);
+    */
     return state;
 }
 
 
 Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt)
 {
-    double h = numeric_limits<double>::epsilon()*10e2;
+    double h = sqrt(numeric_limits<double>::epsilon())*norme2(state);
     Mat<double> var( (double)0, state.getLine(), state.getColumn());
     var.set( h, 1,1);
-    Mat<double> G(motion_bicycle3(state, command, dt) - motion_bicycle3(state+var, command,dt));
+    Mat<double> G(motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt));
     
     for(int i=2;i<=state.getLine();i++)
     {
         var.set( (double)0, i-1,1);
         var.set( h, i,1);
-        G = operatorL(G, motion_bicycle3(state, command, dt) - motion_bicycle3(state+var, command,dt) );
+        G = operatorL(G, motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt) );
     }       
     
     
-    return (1.0/h)*G;
+    return (1.0/(2*h))*G;
 }
 
 Mat<double> jmotion_bicycle3_command(Mat<double> state, Mat<double> command, double dt)
 {
-    double h = numeric_limits<double>::epsilon()*10e2;
+    double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
     Mat<double> var( (double)0, command.getLine(), command.getColumn());
     var.set( h, 1,1);
     Mat<double> G(motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt));
@@ -312,7 +333,7 @@
     {
         var.set( (double)0, i-1,1);
         var.set( h, i,1);
-        G = operatorL(G, motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt) );
+        G = operatorL(G, motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt) );        
     }       
     
     
@@ -321,7 +342,7 @@
 
 Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
 {
-    double h = numeric_limits<double>::epsilon()*10e2;
+    double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
     Mat<double> var((double)0, state.getLine(), state.getColumn());
     var.set( h, 1,1);
     Mat<double> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
@@ -335,6 +356,23 @@
     
     
     return (1.0/(2*h))*H;
+    /*
+    double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
+    Mat<double> var((double)0, state.getLine(), state.getColumn());
+    var.set( h, 1,1);
+    Mat<double> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
+        
+    for(int i=2;i<=state.getLine();i++)
+    {
+        var.set( (double)0, i-1,1);
+        var.set( h, i,1);
+        H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
+        
+    }       
+    
+    
+    return (1.0/(2*h))*H;
+    */
 }
 
 bool setPWM(PwmOut *servo,float p)