AHRS
Dependencies: Eigen
Diff: EKF_RPY.cpp
- Revision:
- 23:71996bfe68eb
- Parent:
- 22:495a419e474c
- Child:
- 24:7fe0935b3bd2
- Child:
- 29:cd963a6d31c5
diff -r 495a419e474c -r 71996bfe68eb EKF_RPY.cpp --- 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)