Diff: EKF_RP.cpp

Revision:

17:1d98928f7681

Parent:

16:c39e084f05ed

Child:

18:f374ca9a3e75

--- a/EKF_RP.cpp	Thu Oct 24 06:43:17 2019 +0000
+++ b/EKF_RP.cpp	Fri Oct 25 08:44:32 2019 +0000
@@ -59,16 +59,19 @@
     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;
+    /* 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;
     
     /* 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()