Diff: EKF_RPY.h

Revision:

21:31e01d3e0143

Child:

22:495a419e474c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EKF_RPY.h	Mon Oct 21 17:14:27 2019 +0000
@@ -0,0 +1,64 @@
+#ifndef EKF_RPY_H_
+#define EKF_RPY_H_
+
+#include <mbed.h>
+#include "Eigen/Dense.h"
+
+using namespace Eigen;
+
+class EKF_RPY
+{
+public:
+
+    EKF_RPY(float Ts, float mx0, float my0, float mz0);
+    EKF_RPY(float Ts);
+    void set_m0(float,float,float);
+
+    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 Ts;
+    float rho;
+    Matrix <float, 4, 1>  var_gy;
+    Matrix <float, 8, 1>  var_fx; 
+
+    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 calc_F();
+    void calc_H();
+    void initialize_R();
+    void initialize_Q();
+    void calc_Q();
+    
+    Matrix <float, 8, 1> fxd();
+    Matrix <float, 4, 1> gy();
+    
+};
+
+#endif
\ No newline at end of file