![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.cpp
- Revision:
- 21:aab1ac72095b
- Parent:
- 20:0227c82a78c2
- Child:
- 22:4148af9e3d81
diff -r 0227c82a78c2 -r aab1ac72095b EKF_RP.cpp --- a/EKF_RP.cpp Mon Oct 28 12:14:59 2019 +0000 +++ b/EKF_RP.cpp Mon Jan 06 10:20:10 2020 +0000 @@ -13,7 +13,7 @@ rho = 1.0f; kv = 0.5f; /* k1/m */ g = 9.81f; - scale_P0 = 1000000.0f; + scale_P0 = 1000.0f; reset(); } @@ -42,12 +42,12 @@ float EKF_RP::get_est_state(uint8_t i) { - /* x = [ang; v; b_g] = [0: phi + /* x = [ang; b_g; v] = [0: phi 1: theta - 2: vx - 3: vy - 4: b_gx - 5: b_gy] */ + 2: b_gx + 3: b_gy + 4: vx + 5: vy] */ return x(i); }