Diff: EKF_RPY.h

Revision:

0:a0e9705be9c4

Child:

7:bcbcc23983de

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EKF_RPY.h	Fri Oct 18 14:55:53 2019 +0000
@@ -0,0 +1,65 @@
+#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
\ No newline at end of file