Diff: EKF_RP.cpp

Revision:

13:2e03d9c03409

Parent:

9:be2e1d2f683d

Child:

15:53485bd1ff28

--- a/EKF_RP.cpp	Tue Oct 22 19:37:32 2019 +0000
+++ b/EKF_RP.cpp	Tue Oct 22 20:21:04 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,14 @@
     
     x = fxd();
     P = F * P * F.transpose() + Q;
-    
-    K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
+    e = y - gy();
     
-    x = x + K * (y - gy());
-    P = (I - K * H) * P;
+    /* only valid if R is diagonal (uncorrelated noise) */
+    for(uint8_t i = 0; i < 4; 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()