File content as of revision 41:c04c2ec37aad:

/*KalmanFilter*/
#include "EKF.h"
#include "Odometry.h"

template<typename T>
Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt = (T)0.5);
template<typename T>
Mat<T> sensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5 );
template<typename T>
Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt = 0.5);
template<typename T>
Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt = 0.5);
template<typename T>
Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5);
template<typename T>
bool setPWM(PwmOut *servo,T p);
int reduc = 16;

extern Serial logger;

/*---------------------------------------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------------------------------------*/

template<typename T>
class Asserv
{
    private :  
    
    Odometry* odometry;      
    EKF<T> instanceEKF;
    int nbrstate;
    int nbrcontrol;
    int nbrobs;
    T dt;
    
    T stdnoise;
    Mat<T> X;
    Mat<T> dX;
    Mat<T> initX;
    Mat<T> z;
    Mat<T> u;
    bool extended;
    bool filterOn;
    
    Mat<T> ki;
    Mat<T> kp;
    Mat<T> kd;
    
    public :
        
    T phi_r;
    T phi_l;
    T phi_max;  
    bool execution;
    bool isarrived;  
    
    Asserv(Odometry* odometry)
    {        
        /*Odometry*/
        this->odometry = odometry;
        phi_max = (T)1.0;                       
        
        /*KalmanFilter*/
        //double phi_max = 100;
        /*en millimetres*/        
        
        nbrstate = 5;
        nbrcontrol = 2;
        nbrobs = 5;
        dt = (T)0.05;
        stdnoise = (T)0.05;
    
        initX = Mat<T>((T)0, nbrstate, 1);  
        initX.set( (T)0, 3,1);
        X = initX;
        
        extended = true;
        filterOn = true;
        
        instanceEKF = EKF<T>(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
        
        instanceEKF.initMotion(motion_bicycle3);
        instanceEKF.initSensor(sensor_bicycle3);
        instanceEKF.initJMotion(jmotion_bicycle3);
        //instanceEKF.initJMotionCommand(jmotion_bicycle3_command);
        instanceEKF.initJSensor(jsensor_bicycle3);
        
        /*desired State : (x y theta phiright phileft)*/
        dX = Mat<T>((T)0, nbrstate, 1);
        dX.set( (T)0, 1,1);
        dX.set( (T)0, 2,1);
        dX.set( (T)0, 3,1);
        dX.set( (T)0, 4,1);
        dX.set( (T)0, 5,1);    
        
        ki = Mat<T>((T)0, nbrcontrol, nbrstate);        
        kp = Mat<T>((T)0, nbrcontrol, nbrstate);
        kd = Mat<T>((T)0, nbrcontrol, nbrstate);
        //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
        
        for(int i=1;i<=nbrstate;i++)
        {
            kp.set( (T)0.01, i, i);
            kd.set( (T)0.0001, i, i);
            ki.set( (T)0.0001, i, i);
        }   
        
        instanceEKF.setKi(ki);
        instanceEKF.setKp(kp);
        instanceEKF.setKd(kd);
        //instance.setKdd(kdd);
        
        u = Mat<T>(transpose( instanceEKF.getCommand()) );
        
        /*Observations*/
        /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
        z = Mat<T>((T)0,5,1);
        this->measurementCallback(&z);    
        
        /*----------------------------------------------------------------------------------------------*/
        isarrived = true;
        execution = false;
            
    }
    
    ~Asserv()
    {
        
    }
    
    void setGoal(T x, T y, T theta)
    {
        dX.set(x,1,1);
        dX.set(y,2,1);
        dX.set(theta,3,1);
        dX.set((T)0,4,1);
        dX.set((T)0,5,1);
        execution = true;
        isarrived = false;
        
    }
    
    bool isArrived() { return isarrived;}        
    
    void stop()
    {
        execution = false;
    }
    
    void update(T deltat)
    {
        if(execution)
        {            
            dt = deltat;        
            /*Asservissement*/        
            logger.printf("1\r\n");
            this->measurementCallback(&z);
            logger.printf("2\r\n");                        
            instanceEKF.measurement_Callback( z, dX, true );
            logger.printf("3\r\n"); 
            instanceEKF.state_Callback();
            //instance.setX(z);
            X = instanceEKF.getX();
            X.afficherMblue();
            
            //instance.computeCommand(dX, (double)dt, -2);                    
            instanceEKF.computeCommand(dX, (T)dt, -2);                    
            phi_r = instanceEKF.getCommand().get(1,1);
            phi_l = instanceEKF.getCommand().get(2,1);                
        }
        else
        {
            phi_r = (T)0;
            phi_l = (T)0;
        }
    }
    
    void measurementCallback( Mat<T>* z)
    {
        z->set( (T)/*conversionUnitée mm */this->odometry->getX(), 1,1);
        z->set( (T)/*conversionUnitée mm*/this->odometry->getY(), 2,1);
        T theta = (T)this->odometry->getTheta();
        theta = atan21(sin(theta),cos(theta));
        z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);   
        T vx = (T)this->odometry->getVx();
        T vy = (T)this->odometry->getVy();
        z->set( sqrt(vx*vx+vy*vy),4,1);
        z->set( (T)odometry->getW(),5,1); 
        
        //z->afficherM();
    }            
    
    Mat<T> getX()
    {
        return X;
    }
    
    T getPhiR()
    {
        return phi_r;
    }
    
    T getPhiL()
    {
        return phi_l;
    }
    
    T getPhiMax()
    {
        return phi_max;
    }
    
    
};    

template<typename T>
bool setPWM(PwmOut *servo,T p)
{
    if(p <= (T)1.0 && p >= (T)0.0)
    {
        servo->write((float)p);        
        return true;
    }
    
    return false;
}

template<typename T>
Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt)
{
    Mat<T> bicycle(3,1);
    bicycle.set((T)36, 1,1);  /*radius*/
    bicycle.set((T)36, 2,1);
    bicycle.set((T)220, 3,1);   /*entre-roue*/
    Mat<T> 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));
    T v = state.get(4,1);
    T w = state.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);
    
    T angle = (r.get(3,1) + dt*w);
    if( angle < -PI)
    {
        angle = angle - PI*ceil(angle/PI);
    }
    else if( angle > PI)
    {
        angle = angle - PI*floor(angle/PI);
    }
    
    r.set( atan21(sin(angle), cos(angle)), 3,1);
    
        
    /*----------------------------------------*/
    /*Modele du moteur*/
    /*----------------------------------------*/
    //double r1 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1));
    //double r2 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1));   
    T r1 = bicycle.get(1,1)/2*(command.get(1,1)+command.get(2,1));
    T r2 = bicycle.get(1,1)/bicycle.get(3,1)*(command.get(1,1)-command.get(2,1));
    
    
    r.set( r1, 4,1);
    r.set( r2, 5,1);    
    
    
    /*----------------------------------------*/
    /*----------------------------------------*/    
    
    return r;
}

template<typename T>
Mat<T> sensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt)
{    
    /*
    double angle = state.get(3,1);
    if( angle < -PI)
    {
        angle = angle - PI*ceil(angle/PI);
    }
    else if( angle > PI)
    {
        angle = angle - PI*floor(angle/PI);
    }
    
    state.set( atan21(sin(angle), cos(angle)), 3,1);
    */
    return state;
}

template<typename T>
Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt)
{
    T h = sqrt(numeric_limits<T>::epsilon())*norme2(state)+ pow(numeric_limits<T>::epsilon(), (T)0.5);
    Mat<T> var( (T)0, state.getLine(), state.getColumn());
    var.set( h, 1,1);
    Mat<T> G(motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt));
    
    for(int i=2;i<=state.getLine();i++)
    {
        var.set( (T)0, i-1,1);
        var.set( h, i,1);
        G = operatorL(G, motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt) );
    }       
    
    
    return ((T)1.0/(2*h))*G;
}

template<typename T>
Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt)
{
    T h = pow(numeric_limits<T>::epsilon()), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
    Mat<T> var( (T)0, command.getLine(), command.getColumn());
    var.set( h, 1,1);
    Mat<T> G(motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt));
    
    for(int i=2;i<=command.getLine();i++)
    {
        var.set( (T)0, i-1,1);
        var.set( h, i,1);
        G = operatorL(G, motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt) );        
    }       
    
    
    return (1.0/(2*h))*G;
}


template<typename T>
Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt)
{
    T h = pow(numeric_limits<T>::epsilon(), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
    Mat<T> var((T)0, state.getLine(), state.getColumn());
    var.set( h, 1,1);
    Mat<T> 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( (T)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 ((T)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;
    */
}