altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
Diff: EKF_RPY.cpp
- Revision:
- 20:0227c82a78c2
- Parent:
- 19:ccb6fc8bf872
- Child:
- 21:aab1ac72095b
diff -r ccb6fc8bf872 -r 0227c82a78c2 EKF_RPY.cpp --- 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()