altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Diff: EKF_RPY.cpp

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