Diff: EKF_RPY.cpp

Revision:

15:53485bd1ff28

Parent:

13:2e03d9c03409

Child:

16:c39e084f05ed

--- a/EKF_RPY.cpp	Wed Oct 23 09:59:59 2019 +0000
+++ b/EKF_RPY.cpp	Thu Oct 24 06:32:00 2019 +0000
@@ -79,12 +79,16 @@
     P = F * P * F.transpose() + Q;
     e = y - gy();
     
+    K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
+    x = x + K * e;
+    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;
-    }
+    // 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()