![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RPY.cpp
- Revision:
- 22:4148af9e3d81
- Parent:
- 21:aab1ac72095b
--- a/EKF_RPY.cpp Mon Jan 06 10:20:10 2020 +0000 +++ b/EKF_RPY.cpp Mon Jan 27 09:24:35 2020 +0000 @@ -27,7 +27,8 @@ /* [n_acc; n_mag] */ var_gy << 40.0f, 40.0f, 1.0f, 1.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; } @@ -63,11 +64,11 @@ /* x = [ang; v; b_g] = [0: phi 1: theta 2: psi - 3: vx - 4: vy - 5: b_gx - 6: b_gy - 7: b_gz] */ + 3: b_gx + 4: b_gy + 5: b_gz + 6: vx + 7: vy] */ return x(i); }