Diff: main.cpp

Revision:

3:573a0dc8383f

Parent:

0:41149573d577

Child:

4:4025c071b207

--- a/main.cpp	Sun Sep 28 19:53:31 2014 +0000
+++ b/main.cpp	Thu Oct 02 16:12:25 2014 +0000
@@ -3,18 +3,175 @@
 #include "Odometry.h"
 
 
+/*---------------------------------------------------------------------------------------------------------*/
+/*---------------------------------------------------------------------------------------------------------*/
+/*KalmanFilter*/
+#include "EKF.h"
+Mat<double> motion_bicycle2( Mat<double> state, Mat<double> command, double dt = 0.5);
+Mat<double> sensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5 );
+Mat<double> jmotion_bicycle2( Mat<double> state, Mat<double> command, double dt = 0.5);
+Mat<double> jsensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5);
+void measurementCallback( Mat<double>* z, Odometry* odometry);
+
+Mat<double> bicycle(3,1);
+/*---------------------------------------------------------------------------------------------------------*/
+/*---------------------------------------------------------------------------------------------------------*/
+
+
+
 int main()
 {
+    /*----------------------------------------------------------------------------------------------*/
+    /*Odometry*/
     QEI qei_left(p15,p16,NC,1024,QEI::X4_ENCODING);
     QEI qei_right(p17,p18,NC,1024,QEI::X4_ENCODING);
     
     Odometry odometry(&qei_left,&qei_right,0.07,0.07,0.26);
+    /*----------------------------------------------------------------------------------------------*/
     
+    
+    
+    /*----------------------------------------------------------------------------------------------*/
+    /*KalmanFilter*/
+    
+    /*en millimetres*/
+    bicycle.set((double)100, 1,1);  /*radius*/
+    bicycle.set((double)100, 2,1);
+    bicycle.set((double)66, 3,1);   /*entre-roue*/
+    
+    int nbrstate = 5;
+    int nbrcontrol = 2;
+    int nbrobs = 3;
+    double dt = (double)0.05;
+    double stdnoise = (double)0.5;
+
+    Mat<double> initX((double)0, nbrstate, 1);  
+    initX.set( (double)0, 3,1);
+    
+    bool extended = true;
+    EKF<double> instance(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended);
+    
+    instance.initMotion(motion_bicycle2);
+    instance.initSensor(sensor_bicycle2);
+    instance.initJMotion(jmotion_bicycle2);
+    instance.initJSensor(jsensor_bicycle2);
+    
+    /*desired State : (x y theta phiright phileft)*/
+    Mat<double> dX((double)0, nbrstate, 1);
+    dX.set( (double)30, 1,1);
+    dX.set( (double)20, 2,1);
+    dX.set( (double)PI/2, 3,1);
+    dX.set( (double)0, 4,1);
+    dX.set( (double)0, 5,1);    
+    
+    Mat<double> u(transpose( instance.getCommand()) );
+    
+    /*Observations*/
+    Mat<double> z(3,1);
+    measurementCallback(&z, &odometry);    
+    
+    /*----------------------------------------------------------------------------------------------*/
+    
+    
+    
+    
+    /*----------------------------------------------------------------------------------------------*/
+    /*Serial*/    
     Serial pc(USBTX, USBRX); // tx, rx
+    /*----------------------------------------------------------------------------------------------*/
     
     while(1)
     {
         wait(1);
         pc.printf("%f : %f : %f\n",odometry.getX()*100,odometry.getY()*100,odometry.getTheta()*180/3.14);
+        
+        
+        /*------------------------------------------------------------------------------------------*/
+        /*Asservissement*/
+        measurementCallback(&z, &odometry);                
+        instance.measurement_Callback( z, dX );
+        instance.state_Callback();
+        
+        instance.computeCommand(dX, (double)dt, -1);        
+        pc.printf("command : \n phi_r = %d \n phi_l = %d \n", instance.getCommand().get(1,1), instance.getCommand().get(2,1) );
+         
+        /*------------------------------------------------------------------------------------------*/
+        
     }
 }
+
+void measurementCallback( Mat<double>* z, Odometry* odometry)
+{
+    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);    
+}
+
+Mat<double> motion_bicycle2( Mat<double> state, Mat<double> command, double dt)
+{
+    Mat<double> r(state);
+    double v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
+    //double w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
+    
+    r.set( r.get(1,1) + v*cos(r.get(3,1))*dt, 1,1);
+    r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
+    
+    double angle = (r.get(3,1) + dt/bicycle.get(3,1)*(r.get(4,1)-r.get(5,1)));
+    if( angle < -PI)
+    {
+        angle = angle - PI*ceil(angle/PI);
+    }
+    else if( angle > PI)
+    {
+        angle = angle - PI*floor(angle/PI);
+    }
+    
+    r.set( angle, 3,1);
+    
+    r.set( bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1)), 4,1);
+    r.set( bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1)), 5,1);        
+    
+    return r;
+}
+
+
+Mat<double> sensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
+{
+    return extract(state-d_state, 1,1, 3,1);
+}
+
+Mat<double> jmotion_bicycle2( Mat<double> state, Mat<double> command, double dt)
+{
+    double h = numeric_limits<double>::epsilon()*10e2;
+    Mat<double> var( (double)0, state.getLine(), state.getColumn());
+    var.set( h, 1,1);
+    Mat<double> G(motion_bicycle2(state, command, dt) - motion_bicycle2(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_bicycle2(state, command, dt) - motion_bicycle2(state+var, command,dt) );
+    }       
+    
+    
+    return (1.0/h)*G;
+}
+
+Mat<double> jsensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
+{
+    double h = numeric_limits<double>::epsilon()*10e2;
+    Mat<double> var((double)0, state.getLine(), state.getColumn());
+    var.set( h, 1,1);
+    Mat<double> H(sensor_bicycle2(state, command, d_state, dt) - sensor_bicycle2(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_bicycle2(state, command, d_state, dt) - sensor_bicycle2(state+var, command, d_state, dt) );
+    }       
+    
+    
+    return (1.0/h)*H;
+}
\ No newline at end of file