![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RPY.h
- Revision:
- 7:bcbcc23983de
- Parent:
- 0:a0e9705be9c4
- Child:
- 12:180e09c4ea26
diff -r f9569a07aff5 -r bcbcc23983de EKF_RPY.h --- a/EKF_RPY.h Sun Oct 20 10:20:05 2019 +0000 +++ b/EKF_RPY.h Sun Oct 20 13:16:13 2019 +0000 @@ -3,8 +3,6 @@ #include <mbed.h> #include "Eigen/Dense.h" -#include "Eigen/Core.h" -#include "Eigen/Geometry.h" using namespace Eigen; @@ -33,32 +31,31 @@ float g; float kv; - float wm; float Ts; float rho; - Matrix <float, 4, 1> var_gy; - Matrix <float, 10, 1> var_fx; + Matrix <float, 4, 1> var_gy; + Matrix <float, 8, 1> var_fx; - Matrix <float, 3, 1> u; - Matrix <float, 4, 1> y; - Matrix <float, 10, 1> x; - Matrix <float, 10, 10> F; - Matrix <float, 4, 10> H; - Matrix <float, 10, 10> Q; - Matrix <float, 4, 4> R; - Matrix <float, 10, 10> P; - Matrix <float, 10, 4> K; - Matrix <float, 10, 10> I; + Matrix <float, 3, 1> u; + Matrix <float, 4, 1> y; + Matrix <float, 8, 1> x; + Matrix <float, 8, 8> F; + Matrix <float, 4, 8> H; + Matrix <float, 8, 8> Q; + Matrix <float, 4, 4> R; + Matrix <float, 8, 8> P; + Matrix <float, 8, 4> K; + Matrix <float, 8, 8> I; void update_angles(); - void update_F(); - void update_H(); + void calc_F(); + void calc_H(); void initialize_R(); void initialize_Q(); - void update_Q(); + void calc_Q(); - Matrix <float, 10, 1> fxd(); - Matrix <float, 4, 1> gy(); + Matrix <float, 8, 1> fxd(); + Matrix <float, 4, 1> gy(); };