File content as of revision 1:6b803652d032:

#include "mbed.h"
#include "iostream"
//#include "Eigen/Dense.h"
#include "Eigen/Core.h"
#include "Eigen/Geometry.h"
#include "EKF_RP.h"

using namespace Eigen;

Serial pc(SERIAL_TX, SERIAL_RX);

// EKF_RP ekf_rp(0.02f);    // initialize counter on PB_6 and PB_7

Timer timer; // timer for time measurement
float dt = 0.0f;

uint32_t counter;

Matrix<float, 8, 8> A;
Matrix<float, 8, 1> b;
// Matrix<float, 8, 8> I;

/******************************************************************************/

    void reset();
    float get_est_state(uint8_t i);
    void update(float gyro_x, float gyro_y, float accel_x, float accel_y);
    
    float s1;
    float c1;
    float s2;
    float c2;
    
    float g;
    float kv;
    float Ts;
    float rho;
    Matrix <float, 2, 1>  var_gy;
    Matrix <float, 6, 1>  var_fx; 

    Matrix <float, 2, 1>  u;
    Matrix <float, 2, 1>  y;
    Matrix <float, 6, 1>  x;
    Matrix <float, 6, 6>  F;
    Matrix <float, 2, 6>  H;
    Matrix <float, 6, 6>  Q;
    Matrix <float, 2, 2>  R;
    Matrix <float, 6, 6>  P;
    Matrix <float, 6, 2>  K;
    Matrix <float, 6, 6>  I;
        
    void update_angles();
    void update_F();
    void update_H();
    void initialize_R();
    void initialize_Q();
    void update_Q();
    
    Matrix <float, 6, 1> fxd();
    Matrix <float, 2, 1> gy();
    
/******************************************************************************/

int main()
{
    pc.baud(2000000);

    timer.start();

    counter = 0;
    
    /*
    A << 1, 2, 3, 5, 1, 8,10, 1, 3, 
         4, 5, 6, 8, 4, 2, 1, 9, 4,
         7, 8,10, 5, 6, 8, 4, 5, 1,
         4, 2,10, 8,10, 5, 6, 7, 8, 
         1, 8, 7, 3, 4, 6, 5, 1, 7,
         4, 2, 7, 5, 7, 6, 9, 2, 1,
         5, 5, 1, 7, 4, 2, 1, 1, 9,
         8, 9, 7, 4, 5, 6, 1, 2, 2,
         1, 5, 9, 4, 8, 7, 2, 6, 3;
    b << 3,
         3,
         4,
         2,
         7,
         5,
         1,
         8,
         1;
    */
    
    A << 1, 2, 3, 5, 1, 8,10, 1, 
         4, 5, 6, 8, 4, 2, 1, 9,
         7, 8,10, 5, 6, 8, 4, 5,
         4, 2,10, 8,10, 5, 6, 7, 
         1, 8, 7, 3, 4, 6, 5, 1,
         4, 2, 7, 5, 7, 6, 9, 2,
         5, 5, 1, 7, 4, 2, 1, 1,
         8, 9, 7, 4, 5, 6, 1, 2;
    b << 3,
         3,
         4,
         2,
         7,
         5,
         1,
         8;
         
    // I.setIdentity();
    
/******************************************************************************/

    // this->Ts = Ts;
    Ts = 0.05;
    /* [n_gyro; n_b_g; n_v] */
    var_fx << 1000, 1000, 20, 20, 10, 10;
    /* [n_acc] */
    var_gy << 40, 40;
    rho = 5000;
    kv = 0.5;       /* k1/m */
    g = 9.81;
    reset();

/******************************************************************************/
        
    while(1) {
        
        Matrix<float, 8, 1> x = A.inverse() * b;
        
        // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
        dt = timer.read();
        timer.reset();
        
        pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt);
        pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2));

        counter++;
        
        wait_us(1000000);
    }
}

void reset()
{   
    u.setZero();
    y.setZero();
    x.setZero();
    update_F();
    update_H();
    initialize_Q();
    initialize_R();
    P = Q;
    K.setZero();
    I.setIdentity();
}

void update(float gyro_x, float gyro_y, float accel_x, float accel_y)
{
    u << gyro_x, gyro_y;
    y << accel_x, accel_y;
    
    update_F();
    // update_H();  /* H remains constant */
    update_Q();
    
    x = fxd();
    P = F * P * F.transpose() + Q;
    
    K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
    
    x = x + K * (y - gy());
    P = (I - K * H) * P;
}

float get_est_state(uint8_t i)
{   
    // x = [ang; b_g; v] 
    return x(i);
}

void update_angles()
{
    s1 = sin(x(0));
    c1 = cos(x(0));
    s2 = sin(x(1));
    c2 = cos(x(1));   
}

void update_F()
{
    F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1), -Ts, -(Ts*s1*s2)/c2,         0,         0,
                    -Ts*s1*(u(1) - x(3)),                                  1,   0,         -Ts*c1,         0,         0,
                                       0,                                  0,   1,              0,         0,         0,
                                       0,                                  0,   0,              1,         0,         0,
                                       0,                            Ts*c2*g,   0,              0, 1 - Ts*kv,         0,
                             -Ts*c1*c2*g,                         Ts*g*s1*s2,   0,              0,         0, 1 - Ts*kv;
}

void update_H()
{
    H << 0, 0, 0, 0, -kv,   0,
         0, 0, 0, 0,   0, -kv;
}

void initialize_R()
{
    R <<  rho*var_gy(0)/Ts,                0,
                         0, rho*var_gy(1)/Ts;
}

void initialize_Q()
{   
    Q << Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)), (Ts*c1*s1*s2*var_fx(1))/c2,            0,            0,            0,            0,
                               (Ts*c1*s1*s2*var_fx(1))/c2,         Ts*c1*c1*var_fx(1),            0,            0,            0,            0,
                                                        0,                          0, Ts*var_fx(2),            0,            0,            0,
                                                        0,                          0,            0, Ts*var_fx(3),            0,            0,
                                                        0,                          0,            0,            0, Ts*var_fx(4),            0,
                                                        0,                          0,            0,            0,            0, Ts*var_fx(5);
}

void update_Q()
{   
    Q(0,0) = Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2));
    Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
    Q(1,0) = Q(0,1);
    Q(1,1) = Ts*c1*c1*var_fx(1);
}

Matrix <float, 6, 1>  fxd()
{
    Matrix <float, 6, 1> retval;
    retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
                                      x(1) + Ts*c1*(u(1) - x(3)),
                                                            x(2),
                                                            x(3),
                                      x(4) + Ts*(g*s2 - kv*x(4)),
                                   x(5) - Ts*(kv*x(5) + c2*g*s1);
    return retval;
}

Matrix <float, 2, 1> gy()
{
    Matrix <float, 2, 1> retval;
    retval << -kv*x(4),
              -kv*x(5);
    return retval;
}