![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.cpp
- Revision:
- 1:6b803652d032
- Parent:
- 0:a0e9705be9c4
- Child:
- 4:e50e18eac72b
--- a/EKF_RP.cpp Fri Oct 18 14:55:53 2019 +0000 +++ b/EKF_RP.cpp Fri Oct 18 21:15:52 2019 +0000 @@ -58,10 +58,10 @@ void EKF_RP::update_angles() { - s1 = sinf(x(0)); - c1 = cosf(x(0)); - s2 = sinf(x(1)); - c2 = cosf(x(1)); + s1 = sin(x(0)); + c1 = cos(x(0)); + s2 = sin(x(1)); + c2 = cos(x(1)); } void EKF_RP::update_F() @@ -90,10 +90,10 @@ { Q << Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)), (Ts*c1*s1*s2*var_fx(1))/c2, 0, 0, 0, 0, (Ts*c1*s1*s2*var_fx(1))/c2, Ts*c1*c1*var_fx(1), 0, 0, 0, 0, - 0, 0, Ts*var_fx(3), 0, 0, 0, - 0, 0, 0, Ts*var_fx(4), 0, 0, - 0, 0, 0, 0, Ts*var_fx(5), 0, - 0, 0, 0, 0, 0, Ts*var_fx(6); + 0, 0, Ts*var_fx(2), 0, 0, 0, + 0, 0, 0, Ts*var_fx(3), 0, 0, + 0, 0, 0, 0, Ts*var_fx(4), 0, + 0, 0, 0, 0, 0, Ts*var_fx(5); } void EKF_RP::update_Q()