![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.cpp
- Revision:
- 22:4148af9e3d81
- Parent:
- 21:aab1ac72095b
- Child:
- 24:e5188a2d72ca
diff -r aab1ac72095b -r 4148af9e3d81 EKF_RP.cpp --- a/EKF_RP.cpp Mon Jan 06 10:20:10 2020 +0000 +++ b/EKF_RP.cpp Mon Jan 27 09:24:35 2020 +0000 @@ -11,7 +11,8 @@ /* [n_acc] */ var_gy << 40.0f, 40.0f; rho = 1.0f; - kv = 0.5f; /* k1/m */ + // kv = 0.5f; /* QK3 k1/m */ + kv = 0.26f; /* QK4 k1/m */ g = 9.81f; scale_P0 = 1000.0f; reset(); @@ -42,12 +43,12 @@ float EKF_RP::get_est_state(uint8_t i) { - /* x = [ang; b_g; v] = [0: phi + /* x = [ang; v; b_g] = [0: phi 1: theta - 2: b_gx - 3: b_gy - 4: vx - 5: vy] */ + 2: vx + 3: vy + 4: b_gx + 5: b_gy] */ return x(i); }