File content as of revision 24:e5188a2d72ca:

#include "EKF_RP.h"

using namespace std;
using namespace Eigen;

EKF_RP::EKF_RP(float Ts)
{
    this->Ts = Ts;
    set_para();
    reset();
}

EKF_RP::~EKF_RP() {}
    
void EKF_RP::set_para()
{   
    // kv = 0.5f;       /* QK3 k1/m */
    kv = 0.25f;       /* QK4*/
    g  = 9.81f;
    wg = 0.5026549f; // 2*pi*0.08
    wa = 1.2566371f; // 2*pi*0.2
    scale_P0 = 100.0f;
    Q <<  4.0000265e-03f,  0.0000000e+00f, -1.9799947e-06f,  0.0000000e+00f,  0.0000000e+00f, -3.9172179e-04f,  0.0000000e+00f,  0.0000000e+00f, 
        0.0000000e+00f,  4.0000265e-03f,  0.0000000e+00f, -1.9799947e-06f,  3.9172179e-04f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f, 
       -1.9799947e-06f,  0.0000000e+00f,  1.9800281e-04f,  0.0000000e+00f,  0.0000000e+00f,  1.2981776e-07f,  0.0000000e+00f,  0.0000000e+00f, 
        0.0000000e+00f, -1.9799947e-06f,  0.0000000e+00f,  1.9800281e-04f, -1.2981776e-07f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f, 
        0.0000000e+00f,  3.9172179e-04f,  0.0000000e+00f, -1.2981776e-07f,  2.5008971e-04f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f, 
       -3.9172179e-04f,  0.0000000e+00f,  1.2981776e-07f,  0.0000000e+00f,  0.0000000e+00f,  2.5008971e-04f,  0.0000000e+00f,  0.0000000e+00f, 
        0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  1.9505688e-04f,  0.0000000e+00f, 
        0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  0.0000000e+00f,  1.9505688e-04f;
    R <<  3.5000000e+02f,  0.0000000e+00f, 
        0.0000000e+00f,  3.5000000e+02f;
}

void EKF_RP::reset()
{   
    u.setZero();
    y.setZero();
    x.setZero();
    update_angles();
    calc_F();
    calc_H();
    K.setZero();
    I.setIdentity();
    e.setZero();
    P = scale_P0 * I;
}

void EKF_RP::increase_diag_P()
{
    P = P + scale_P0 * I;      
}

float EKF_RP::get_est_state(uint8_t i)
{   
    /* x = [ang; b_g; v; b_a] = [0: phi
                                                             1: theta
                                                             2: b_gx
                                                             3: b_gy
                                                             4: vx
                                                             5: vy]
                                                             6: b_ax]
                                                             7: b_ay] */
    return x(i);
}

void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y)
{
    u << gyro_x, gyro_y;
    y << accel_x, accel_y;
    update_angles();
    
    calc_F();
    // calc_H(); /* H remains constant */
    
    x = fxd();
    P = F * P * F.transpose() + Q;
    e = y - gy();
    
    /* RP   6 states on PES-board: inversion faster  184 mus <  207 mus recursion, mbed online */
    /* RPY  8 states on PES-board: inversion faster  356 mus <  432 mus recursion, mbed online */
    /* RP   6 states on  QK-board: inversion        1204 mus                     , Keil muVision5 */
    /* RPY  8 states on  QK-board: inversion        2582 mus                     , Keil muVision5 */
    
    /* RP   8 states on PES-board: inversion faster  284 mus                     , mbed online */
    /* RP   8 states on  qk-board: inversion faster 1907 mus < 2318 mus recursion, Keil muVision5 */
    /* RPY 12 states on  qk-board: inversion faster 5295 mus < 8575 mus recursion, Keil muVision5 */
    
    /* inversion */
    K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
    x = x + K * e;
    P -= K * H * P; // (I - K * H) * P;
    
    /* recursion: only valid if R is diagonal (uncorrelated noise) */
    /*
    for(uint8_t i = 0; i < 2; i++)  {
            K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
            x = x + K.col(i) * e(i);
            P -= K.col(i) * H.row(i) * P; // P = (I - K.col(i) * H.row(i)) * P;
    }
    */
    
}

void EKF_RP::update_angles()
{
    s1 = sinf(x(0));
    c1 = cosf(x(0));
    s2 = sinf(x(1));
    c2 = cosf(x(1));   
}

void EKF_RP::calc_F()
{
    F << Ts*c1*s2*(u(1) - x(3))/c2 + 1.0f, Ts*s1*(u(1) - x(3))/(c2*c2),  -Ts, -Ts*s1*s2/c2,         0.0f,         0.0f,         0.0f,         0.0f,
                                     -Ts*s1*(u(1) - x(3)),                        1.0f, 0.0f,       -Ts*c1,         0.0f,         0.0f,         0.0f,         0.0f,
                                                                     0.0f,                        0.0f, 1.0f,         0.0f,         0.0f,         0.0f,         0.0f,         0.0f,
                                                                     0.0f,                        0.0f, 0.0f,         1.0f,         0.0f,         0.0f,         0.0f,         0.0f,
                                                                     0.0f,                     Ts*c2*g, 0.0f,         0.0f, 1.0f - Ts*kv,         0.0f,         0.0f,         0.0f,
                                                        -Ts*c1*c2*g,                  Ts*g*s1*s2, 0.0f,         0.0f,         0.0f, 1.0f - Ts*kv,         0.0f,         0.0f,
                                                                     0.0f,                        0.0f, 0.0f,         0.0f,         0.0f,         0.0f, 1.0f - Ts*wa,         0.0f,
                                                                     0.0f,                        0.0f, 0.0f,         0.0f,         0.0f,         0.0f,         0.0f, 1.0f - Ts*wa;
}

void EKF_RP::calc_H()
{
    H << 0.0f, 0.0f, 0.0f, 0.0f,  -kv, 0.0f, 1.0f, 0.0f,
             0.0f, 0.0f, 0.0f, 0.0f, 0.0f,  -kv, 0.0f, 1.0f;
}

Matrix <float, 8, 1>  EKF_RP::fxd()
{
    Matrix <float, 8, 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),
                                                                                         x(6) - Ts*x(6)*wa,
                                                                                         x(7) - Ts*x(7)*wa;
    return retval;
}

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