Diff: EKF_RPY.cpp

Revision:

13:2e03d9c03409

Parent:

12:180e09c4ea26

Child:

15:53485bd1ff28

--- a/EKF_RPY.cpp	Tue Oct 22 19:37:32 2019 +0000
+++ b/EKF_RPY.cpp	Tue Oct 22 20:21:04 2019 +0000
@@ -23,10 +23,10 @@
 void EKF_RPY::set_para()
 {   
     /* [n_gyro; n_v; n_b_g] */
-    var_fx << 0.01f, 0.01f, 0.01f, 0.0001f, 0.0001f, 0.0001f, 0.0001f, 0.0001f;
+    var_fx << 0.1f, 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f, 0.002f;
     /* [n_acc; n_mag] */
-    var_gy << 45.0f, 45.0f, 0.1f, 0.1f;
-    rho = 0.5f;
+    var_gy << 40.0f, 40.0f, 1.0f, 1.0f;
+    rho = 1.0f;
     kv = 0.5f;       /* k1/m */
     g = 9.81f;
 }
@@ -44,6 +44,7 @@
     P = Q;
     K.setZero();
     I.setIdentity();
+    e.setZero();
 }
 
 void EKF_RPY::set_m0(float mx0, float my0, float mz0)
@@ -76,11 +77,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_RPY::update_angles()