altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

Diff: EKF_RPY.cpp

Revision:
19:ccb6fc8bf872
Parent:
18:f374ca9a3e75
Child:
20:0227c82a78c2
diff -r f374ca9a3e75 -r ccb6fc8bf872 EKF_RPY.cpp
--- a/EKF_RPY.cpp	Mon Oct 28 11:31:48 2019 +0000
+++ b/EKF_RPY.cpp	Mon Oct 28 12:08:15 2019 +0000
@@ -29,6 +29,7 @@
     rho = 1.0f;
     kv = 0.5f;       /* k1/m */
     g = 9.81f;
+    scale_P0 = 1000000.0f;
 }
 
 void EKF_RPY::reset()
@@ -41,10 +42,16 @@
     calc_H();
     initialize_Q();
     initialize_R();
-    P = 1000000.0f * ( F * P * F.transpose() + Q );
     K.setZero();
     I.setIdentity();
     e.setZero();
+    P.setZero();
+    increase_diag_P();
+}
+
+void EKF_RPY::increase_diag_P()
+{
+    P = P + scale_P0 * I;
 }
 
 void EKF_RPY::set_m0(float mx0, float my0, float mz0)