Diff: EKF_RP.cpp

Revision:

22:495a419e474c

Parent:

21:31e01d3e0143

Child:

23:71996bfe68eb

--- a/EKF_RP.cpp	Mon Oct 21 17:14:27 2019 +0000
+++ b/EKF_RP.cpp	Mon Oct 28 07:52:43 2019 +0000
@@ -7,10 +7,10 @@
 {
     this->Ts = Ts;
     /* [n_gyro; n_b_g; n_v] */
-    var_fx << 0.01f, 0.01f, 0.0001f, 0.0001f, 0.0001f, 0.0001f;
+    var_fx << 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f;
     /* [n_acc] */
-    var_gy << 45.0f, 45.0f;
-    rho = 0.5f;
+    var_gy << 40.0f, 40.0f;
+    rho = 1.0f;
     kv = 0.5f;       /* k1/m */
     g = 9.81f;
     reset();
@@ -31,6 +31,7 @@
     P = Q;
     K.setZero();
     I.setIdentity();
+    e.setZero();
 }
 
 float EKF_RP::get_est_state(uint8_t i)
@@ -56,11 +57,21 @@
     
     x = fxd();
     P = F * P * F.transpose() + Q;
+    e = y - gy();
     
+    /* inversion faster 184 mus < 207 mus recursion */
     K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
+    x = x + K * e;
+    P = (I - K * H) * P;
     
-    x = x + K * (y - gy());
-    P = (I - K * H) * P;
+    /* only valid if R is diagonal (uncorrelated noise) */
+    /*
+    for(uint8_t i = 0; i < 2; i++)  {
+        K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
+        x = x + K.col(i) * e(i);
+        P = (I - K.col(i)*H.row(i)) * P;
+    }
+    */
 }
 
 void EKF_RP::update_angles()