altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.cpp
- Revision:
- 7:bcbcc23983de
- Parent:
- 6:f9569a07aff5
- Child:
- 8:a72cd6356bab
diff -r f9569a07aff5 -r bcbcc23983de EKF_RP.cpp --- a/EKF_RP.cpp Sun Oct 20 10:20:05 2019 +0000 +++ b/EKF_RP.cpp Sun Oct 20 13:16:13 2019 +0000 @@ -35,7 +35,12 @@ float EKF_RP::get_est_state(uint8_t i) { - /* x = [ang; b_g; v] = [phi; theta; b_gx; b_gy; vx; vy] */ + /* x = [ang; v; b_g] = [0: phi + 1: theta + 2: vx + 3: vy + 4: b_gx + 5: b_gy] */ return x(i); } @@ -58,11 +63,6 @@ P = (I - K * H) * P; } -float EKF_RP::read_Q(uint8_t i, uint8_t j) -{ - return Q(i,j); -} - void EKF_RP::update_angles() { s1 = sinf(x(0)); @@ -73,12 +73,12 @@ void EKF_RP::calc_F() { - F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1.0f, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1.0f), -Ts, -(Ts*s1*s2)/c2, 0.0f, 0.0f, - -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, - 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f, - -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv; + F << Ts*c1*s2*(u(1) - x(3))/c2 + 1.0f, Ts*s1*(u(1) - x(3))/(c2*c2), -Ts, -Ts*s1*s2/c2, 0.0f, 0.0f, + -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f, + -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv; } void EKF_RP::calc_H()