altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
Diff: EKF_RPY.cpp
- Revision:
- 18:f374ca9a3e75
- Parent:
- 17:1d98928f7681
- Child:
- 19:ccb6fc8bf872
diff -r 1d98928f7681 -r f374ca9a3e75 EKF_RPY.cpp --- a/EKF_RPY.cpp Fri Oct 25 08:44:32 2019 +0000 +++ b/EKF_RPY.cpp Mon Oct 28 11:31:48 2019 +0000 @@ -41,7 +41,7 @@ calc_H(); initialize_Q(); initialize_R(); - P = Q; + P = 1000000.0f * ( F * P * F.transpose() + Q ); K.setZero(); I.setIdentity(); e.setZero();