EKF_RPY.h

00001 #ifndef EKF_RPY_H_
00002 #define EKF_RPY_H_
00003 
00004 #include <mbed.h>
00005 #include "Eigen/Dense.h"
00006 
00007 using namespace Eigen;
00008 
00009 class EKF_RPY
00010 {
00011 public:
00012 
00013     EKF_RPY(float Ts, float mx0, float my0, float mz0);
00014     EKF_RPY(float Ts);
00015 
00016     virtual ~EKF_RPY();
00017     
00018     void set_para();
00019     void reset();
00020     void increase_diag_P();
00021     void set_m0(float mx0, float my0, float mz0);
00022     float get_est_state(uint8_t i);
00023     void update(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float magnet_x, float magnet_y);
00024     
00025 private:
00026 
00027     Matrix <float,  3,  1>  m0;
00028     
00029     float s1;
00030     float c1;
00031     float s2;
00032     float c2;
00033     float s3;
00034     float c3;
00035     
00036     float scale_P0;
00037     float g;
00038     float kv;
00039     float Ts;
00040     float rho;
00041     Matrix <float, 4, 1>  var_gy;
00042     Matrix <float, 8, 1>  var_fx; 
00043 
00044     Matrix <float, 3, 1>  u;
00045     Matrix <float, 4, 1>  y;
00046     Matrix <float, 8, 1>  x;
00047     Matrix <float, 8, 8>  F;
00048     Matrix <float, 4, 8>  H;
00049     Matrix <float, 8, 8>  Q;
00050     Matrix <float, 4, 4>  R;
00051     Matrix <float, 8, 8>  P;
00052     Matrix <float, 8, 4>  K;
00053     Matrix <float, 8, 8>  I;
00054     Matrix <float, 4, 1>  e;
00055         
00056     void update_angles();
00057     void calc_F();
00058     void calc_H();
00059     void initialize_R();
00060     void initialize_Q();
00061     void calc_Q();
00062     
00063     Matrix <float, 8, 1> fxd();
00064     Matrix <float, 4, 1> gy();
00065     
00066 };
00067 
00068 #endif