File content as of revision 24:e5188a2d72ca:

#ifndef EKF_RP_H_
#define EKF_RP_H_

#include <mbed.h>
#include "Eigen/Dense.h"

using namespace Eigen;

class EKF_RP
{
public:

    EKF_RP(float Ts);

    virtual ~EKF_RP();
    
    void  set_para();
    void  reset();
    void  increase_diag_P();
    float get_est_state(uint8_t i);
    void  update(float gyro_x, float gyro_y, float accel_x, float accel_y);
    
private:
            
    float Ts;
    float kv, g, wg, wa;
    float scale_P0;

    float s1, c1, s2, c2;

    Matrix <float, 2, 1>  u;
    Matrix <float, 2, 1>  y;
    Matrix <float, 8, 1>  x;
    Matrix <float, 8, 8>  F;
    Matrix <float, 2, 8>  H;
    Matrix <float, 8, 8>  Q;
    Matrix <float, 2, 2>  R;
    Matrix <float, 8, 8>  P;
    Matrix <float, 8, 2>  K;
    Matrix <float, 8, 8>  I;
    Matrix <float, 2, 1>  e;
            
    void update_angles();
    void calc_F();
    void calc_H();
    
    Matrix <float, 8, 1> fxd();
    Matrix <float, 2, 1> gy();
    
};

#endif