![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RPY.cpp
- Revision:
- 12:180e09c4ea26
- Parent:
- 9:be2e1d2f683d
- Child:
- 13:2e03d9c03409
diff -r 6a7c3795d8fb -r 180e09c4ea26 EKF_RPY.cpp --- a/EKF_RPY.cpp Mon Oct 21 09:20:25 2019 +0000 +++ b/EKF_RPY.cpp Tue Oct 22 19:37:32 2019 +0000 @@ -6,6 +6,22 @@ EKF_RPY::EKF_RPY(float Ts, float mx0, float my0, float mz0) { this->Ts = Ts; + set_para(); + m0 << mx0, my0, mz0; + reset(); +} + +EKF_RPY::EKF_RPY(float Ts) +{ + this->Ts = Ts; + set_para(); + reset(); +} + +EKF_RPY::~EKF_RPY() {} + +void EKF_RPY::set_para() +{ /* [n_gyro; n_v; n_b_g] */ var_fx << 0.01f, 0.01f, 0.01f, 0.0001f, 0.0001f, 0.0001f, 0.0001f, 0.0001f; /* [n_acc; n_mag] */ @@ -13,12 +29,8 @@ rho = 0.5f; kv = 0.5f; /* k1/m */ g = 9.81f; - m0 << mx0, my0, mz0; - reset(); } -EKF_RPY::~EKF_RPY() {} - void EKF_RPY::reset() { u.setZero(); @@ -34,6 +46,11 @@ I.setIdentity(); } +void EKF_RPY::set_m0(float mx0, float my0, float mz0) +{ + m0 << mx0, my0, mz0; +} + float EKF_RPY::get_est_state(uint8_t i) { /* x = [ang; v; b_g] = [0: phi