File content as of revision 0:a0e9705be9c4:

#ifndef EKF_RPY_H_
#define EKF_RPY_H_

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

using namespace Eigen;

class EKF_RPY
{
public:

    EKF_RPY(float Ts, float mx0, float my0, float mz0);

    virtual ~EKF_RPY();
    
    void reset();
    float get_est_state(uint8_t i);
    void update(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float magnet_x, float magnet_y);
    
private:

    Matrix <float,  3,  1>  m0;
    
    float s1;
    float c1;
    float s2;
    float c2;
    float s3;
    float c3;
    
    float g;
    float kv;
    float wm;
    float Ts;
    float rho;
    Matrix <float,  4,  1>  var_gy;
    Matrix <float, 10,  1>  var_fx; 

    Matrix <float,  3,  1>  u;
    Matrix <float,  4,  1>  y;
    Matrix <float, 10,  1>  x;
    Matrix <float, 10, 10>  F;
    Matrix <float,  4, 10>  H;
    Matrix <float, 10, 10>  Q;
    Matrix <float,  4,  4>  R;
    Matrix <float, 10, 10>  P;
    Matrix <float, 10,  4>  K;
    Matrix <float, 10, 10>  I;
        
    void update_angles();
    void update_F();
    void update_H();
    void initialize_R();
    void initialize_Q();
    void update_Q();
    
    Matrix <float, 10,  1> fxd();
    Matrix <float,  4,  1> gy();
    
};

#endif