Diff: EKF_RP.h

Revision:

24:e5188a2d72ca

Parent:

19:ccb6fc8bf872

--- a/EKF_RP.h	Thu Jan 30 14:14:50 2020 +0000
+++ b/EKF_RP.h	Wed Feb 05 15:35:01 2020 +0000
@@ -14,46 +14,37 @@
 
     virtual ~EKF_RP();
     
-    void reset();
-    void increase_diag_P();
+    void  set_para();
+    void  reset();
+    void  increase_diag_P();
     float get_est_state(uint8_t i);
-    void update(float gyro_x, float gyro_y, float accel_x, float accel_y);
+    void  update(float gyro_x, float gyro_y, float accel_x, float accel_y);
     
 private:
-    
-    float s1;
-    float c1;
-    float s2;
-    float c2;
-    
+            
+    float Ts;
+    float kv, g, wg, wa;
     float scale_P0;
-    float g;
-    float kv;
-    float Ts;
-    float rho;
-    Matrix <float, 2, 1>  var_gy;
-    Matrix <float, 6, 1>  var_fx; 
+
+    float s1, c1, s2, c2;
 
     Matrix <float, 2, 1>  u;
     Matrix <float, 2, 1>  y;
-    Matrix <float, 6, 1>  x;
-    Matrix <float, 6, 6>  F;
-    Matrix <float, 2, 6>  H;
-    Matrix <float, 6, 6>  Q;
+    Matrix <float, 8, 1>  x;
+    Matrix <float, 8, 8>  F;
+    Matrix <float, 2, 8>  H;
+    Matrix <float, 8, 8>  Q;
     Matrix <float, 2, 2>  R;
-    Matrix <float, 6, 6>  P;
-    Matrix <float, 6, 2>  K;
-    Matrix <float, 6, 6>  I;
+    Matrix <float, 8, 8>  P;
+    Matrix <float, 8, 2>  K;
+    Matrix <float, 8, 8>  I;
     Matrix <float, 2, 1>  e;
-        
+            
     void update_angles();
     void calc_F();
     void calc_H();
-    void initialize_R();
-    void initialize_Q();
-    void calc_Q();
     
-    Matrix <float, 6, 1> fxd();
+    Matrix <float, 8, 1> fxd();
     Matrix <float, 2, 1> gy();
     
 };