altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

Files at this revision

API Documentation at this revision

Revision 20:0227c82a78c2, committed 2019-10-28

Comitter:
pmic
Date:
Mon Oct 28 12:14:59 2019 +0000
Parent:
19:ccb6fc8bf872
Child:
21:aab1ac72095b
Commit message:
Change reset logic, so we get the same results at the beginning (reset) or arming (increase_diag_P).

Changed in this revision

EKF_RP.cpp Show annotated file Show diff for this revision Revisions of this file
EKF_RPY.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/EKF_RP.cpp	Mon Oct 28 12:08:15 2019 +0000
+++ b/EKF_RP.cpp	Mon Oct 28 12:14:59 2019 +0000
@@ -32,8 +32,7 @@
     K.setZero();
     I.setIdentity();
     e.setZero();
-    P.setZero();
-    increase_diag_P();
+    P = scale_P0 * I;
 }
 
 void EKF_RP::increase_diag_P()

--- a/EKF_RPY.cpp	Mon Oct 28 12:08:15 2019 +0000
+++ b/EKF_RPY.cpp	Mon Oct 28 12:14:59 2019 +0000
@@ -45,8 +45,7 @@
     K.setZero();
     I.setIdentity();
     e.setZero();
-    P.setZero();
-    increase_diag_P();
+    P = scale_P0 * I;
 }
 
 void EKF_RPY::increase_diag_P()