EKF_RP.h

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