Diff: EKF_RPY.cpp

Revision:

16:c39e084f05ed

Parent:

15:53485bd1ff28

Child:

17:1d98928f7681

diff -r 53485bd1ff28 -r c39e084f05ed EKF_RPY.cpp
--- a/EKF_RPY.cpp	Thu Oct 24 06:32:00 2019 +0000
+++ b/EKF_RPY.cpp	Thu Oct 24 06:43:17 2019 +0000
@@ -79,16 +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;
+    // 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()