Diff: EKF_RP.cpp

Revision:

4:e50e18eac72b

Parent:

1:6b803652d032

Child:

5:676cbc33c81b

--- a/EKF_RP.cpp	Fri Oct 18 21:49:59 2019 +0000
+++ b/EKF_RP.cpp	Sun Oct 20 08:27:25 2019 +0000
@@ -23,8 +23,9 @@
     u.setZero();
     y.setZero();
     x.setZero();
-    update_F();
-    update_H();
+    update_angles();
+    calc_F();
+    calc_H();
     initialize_Q();
     initialize_R();
     P = Q;
@@ -32,28 +33,34 @@
     I.setIdentity();
 }
 
+float EKF_RP::get_est_state(uint8_t i)
+{   
+    /* x = [ang; b_g; v] */
+    return x(i);
+}
+
 void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y)
 {
-    // u << gyro_x, gyro_y;
-    // y << accel_x, accel_y;
+    u << gyro_x, gyro_y;
+    y << accel_x, accel_y;
+    update_angles();
     
-    // update_F();
-    // update_H(); /* H remains constant */
-    // update_Q();
+    calc_F();
+    calc_H(); /* H remains constant */
+    calc_Q();
     
-    // x = fxd();
+    x = fxd();
     // P = F * P * F.transpose() + Q;
     
-    // K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
+    K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
     
     // x = x + K * (y - gy());
     // P = (I - K * H) * P;
 }
 
-float EKF_RP::get_est_state(uint8_t i)
+float EKF_RP::read_Q(uint8_t i, uint8_t j)
 {   
-    /* x = [ang; b_g; v] */
-    return x(i);
+    return Q(i,j);
 }
 
 void EKF_RP::update_angles()
@@ -64,7 +71,7 @@
     c2 = cos(x(1));   
 }
 
-void EKF_RP::update_F()
+void EKF_RP::calc_F()
 {
     F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1), -Ts, -(Ts*s1*s2)/c2,         0,         0,
                     -Ts*s1*(u(1) - x(3)),                                  1,   0,         -Ts*c1,         0,         0,
@@ -74,7 +81,7 @@
                              -Ts*c1*c2*g,                         Ts*g*s1*s2,   0,              0,         0, 1 - Ts*kv;
 }
 
-void EKF_RP::update_H()
+void EKF_RP::calc_H()
 {
     H << 0, 0, 0, 0, -kv,   0,
          0, 0, 0, 0,   0, -kv;
@@ -88,17 +95,17 @@
 
 void EKF_RP::initialize_Q()
 {   
-    Q << Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)), (Ts*c1*s1*s2*var_fx(1))/c2,            0,            0,            0,            0,
-                               (Ts*c1*s1*s2*var_fx(1))/c2,         Ts*c1*c1*var_fx(1),            0,            0,            0,            0,
-                                                        0,                          0, Ts*var_fx(2),            0,            0,            0,
-                                                        0,                          0,            0, Ts*var_fx(3),            0,            0,
-                                                        0,                          0,            0,            0, Ts*var_fx(4),            0,
-                                                        0,                          0,            0,            0,            0, Ts*var_fx(5);
+    Q << Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2)), Ts*c1*s1*s2*var_fx(1)/c2,            0,            0,            0,            0,
+                               Ts*c1*s1*s2*var_fx(1)/c2,       Ts*c1*c1*var_fx(1),            0,            0,            0,            0,
+                                                      0,                        0, Ts*var_fx(2),            0,            0,            0,
+                                                      0,                        0,            0, Ts*var_fx(3),            0,            0,
+                                                      0,                        0,            0,            0, Ts*var_fx(4),            0,
+                                                      0,                        0,            0,            0,            0, Ts*var_fx(5);
 }
 
-void EKF_RP::update_Q()
+void EKF_RP::calc_Q()
 {   
-    Q(0,0) = Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2));
+    Q(0,0) = Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2));
     Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
     Q(1,0) = Q(0,1);
     Q(1,1) = Ts*c1*c1*var_fx(1);