Diff: EKF_RPY.h

Revision:

7:bcbcc23983de

Parent:

0:a0e9705be9c4

Child:

12:180e09c4ea26

--- a/EKF_RPY.h	Sun Oct 20 10:20:05 2019 +0000
+++ b/EKF_RPY.h	Sun Oct 20 13:16:13 2019 +0000
@@ -3,8 +3,6 @@
 
 #include <mbed.h>
 #include "Eigen/Dense.h"
-#include "Eigen/Core.h"
-#include "Eigen/Geometry.h"
 
 using namespace Eigen;
 
@@ -33,32 +31,31 @@
     
     float g;
     float kv;
-    float wm;
     float Ts;
     float rho;
-    Matrix <float,  4,  1>  var_gy;
-    Matrix <float, 10,  1>  var_fx; 
+    Matrix <float, 4, 1>  var_gy;
+    Matrix <float, 8, 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;
+    Matrix <float, 3, 1>  u;
+    Matrix <float, 4, 1>  y;
+    Matrix <float, 8, 1>  x;
+    Matrix <float, 8, 8>  F;
+    Matrix <float, 4, 8>  H;
+    Matrix <float, 8, 8>  Q;
+    Matrix <float, 4, 4>  R;
+    Matrix <float, 8, 8>  P;
+    Matrix <float, 8, 4>  K;
+    Matrix <float, 8, 8>  I;
         
     void update_angles();
-    void update_F();
-    void update_H();
+    void calc_F();
+    void calc_H();
     void initialize_R();
     void initialize_Q();
-    void update_Q();
+    void calc_Q();
     
-    Matrix <float, 10,  1> fxd();
-    Matrix <float,  4,  1> gy();
+    Matrix <float, 8, 1> fxd();
+    Matrix <float, 4, 1> gy();
     
 };