Diff: EKF_RP.cpp

Revision:

5:676cbc33c81b

Parent:

4:e50e18eac72b

Child:

6:f9569a07aff5

--- a/EKF_RP.cpp	Sun Oct 20 08:27:25 2019 +0000
+++ b/EKF_RP.cpp	Sun Oct 20 08:59:31 2019 +0000
@@ -7,12 +7,12 @@
 {
     this->Ts = Ts;
     /* [n_gyro; n_b_g; n_v] */
-    var_fx << 1000, 1000, 20, 20, 10, 10;
+    var_fx << 1000.0f, 1000.0f, 20.0f, 20.0f, 10.0f, 10.0f;
     /* [n_acc] */
-    var_gy << 40, 40;
-    rho = 5000;
-    kv = 0.5;       /* k1/m */
-    g = 9.81;
+    var_gy << 40.0f, 40.0f;
+    rho = 5000.0f;
+    kv = 0.5f;       /* k1/m */
+    g = 9.81f;
     reset();
 }
 
@@ -46,16 +46,16 @@
     update_angles();
     
     calc_F();
-    calc_H(); /* H remains constant */
+    // calc_H(); /* H remains constant */
     calc_Q();
     
     x = fxd();
-    // P = F * P * F.transpose() + Q;
+    P = F * P * F.transpose() + Q;
     
     K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
     
-    // x = x + K * (y - gy());
-    // P = (I - K * H) * P;
+    x = x + K * (y - gy());
+    P = (I - K * H) * P;
 }
 
 float EKF_RP::read_Q(uint8_t i, uint8_t j)
@@ -65,42 +65,42 @@
 
 void EKF_RP::update_angles()
 {
-    s1 = sin(x(0));
-    c1 = cos(x(0));
-    s2 = sin(x(1));
-    c2 = cos(x(1));   
+    s1 = sinf(x(0));
+    c1 = cosf(x(0));
+    s2 = sinf(x(1));
+    c2 = cosf(x(1));   
 }
 
 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,
-                                       0,                                  0,   1,              0,         0,         0,
-                                       0,                                  0,   0,              1,         0,         0,
-                                       0,                            Ts*c2*g,   0,              0, 1 - Ts*kv,         0,
-                             -Ts*c1*c2*g,                         Ts*g*s1*s2,   0,              0,         0, 1 - Ts*kv;
+    F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1.0f, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1.0f),  -Ts, -(Ts*s1*s2)/c2,         0.0f,         0.0f,
+                       -Ts*s1*(u(1) - x(3)),                                  1.0f, 0.0f,         -Ts*c1,         0.0f,         0.0f,
+                                       0.0f,                                  0.0f, 1.0f,           0.0f,         0.0f,         0.0f,
+                                       0.0f,                                  0.0f, 0.0f,           1.0f,         0.0f,         0.0f,
+                                       0.0f,                               Ts*c2*g, 0.0f,           0.0f, 1.0f - Ts*kv,         0.0f,
+                                -Ts*c1*c2*g,                            Ts*g*s1*s2, 0.0f,           0.0f,         0.0f, 1.0f - Ts*kv;
 }
 
 void EKF_RP::calc_H()
 {
-    H << 0, 0, 0, 0, -kv,   0,
-         0, 0, 0, 0,   0, -kv;
+    H << 0.0f, 0.0f, 0.0f, 0.0f,  -kv, 0.0f,
+         0.0f, 0.0f, 0.0f, 0.0f, 0.0f,  -kv;
 }
 
 void EKF_RP::initialize_R()
 {
-    R <<  rho*var_gy(0)/Ts,                0,
-                         0, rho*var_gy(1)/Ts;
+    R <<  rho*var_gy(0)/Ts,             0.0f,
+                      0.0f, rho*var_gy(1)/Ts;
 }
 
 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.0f,         0.0f,         0.0f,         0.0f,
+                               Ts*c1*s1*s2*var_fx(1)/c2,       Ts*c1*c1*var_fx(1),         0.0f,         0.0f,         0.0f,         0.0f,
+                                                   0.0f,                     0.0f, Ts*var_fx(2),         0.0f,         0.0f,         0.0f,
+                                                   0.0f,                     0.0f,         0.0f, Ts*var_fx(3),         0.0f,         0.0f,
+                                                   0.0f,                     0.0f,         0.0f,         0.0f, Ts*var_fx(4),         0.0f,
+                                                   0.0f,                     0.0f,         0.0f,         0.0f,         0.0f, Ts*var_fx(5);
 }
 
 void EKF_RP::calc_Q()