altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Diff: EKF_RP.cpp

Revision:
23:71996bfe68eb
Parent:
22:495a419e474c
Child:
24:7fe0935b3bd2
Child:
29:cd963a6d31c5
--- a/EKF_RP.cpp	Mon Oct 28 07:52:43 2019 +0000
+++ b/EKF_RP.cpp	Wed Nov 13 11:40:46 2019 +0000
@@ -13,6 +13,7 @@
     rho = 1.0f;
     kv = 0.5f;       /* k1/m */
     g = 9.81f;
+    scale_P0 = 1000.0f;
     reset();
 }
 
@@ -28,10 +29,15 @@
     calc_H();
     initialize_Q();
     initialize_R();
-    P = Q;
     K.setZero();
     I.setIdentity();
     e.setZero();
+    P = scale_P0 * I;
+}
+
+void EKF_RP::increase_diag_P()
+{
+    P = P + scale_P0 * I;      
 }
 
 float EKF_RP::get_est_state(uint8_t i)