![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RPY.h
- Revision:
- 0:a0e9705be9c4
- Child:
- 7:bcbcc23983de
diff -r 000000000000 -r a0e9705be9c4 EKF_RPY.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EKF_RPY.h Fri Oct 18 14:55:53 2019 +0000 @@ -0,0 +1,65 @@ +#ifndef EKF_RPY_H_ +#define EKF_RPY_H_ + +#include <mbed.h> +#include "Eigen/Dense.h" +#include "Eigen/Core.h" +#include "Eigen/Geometry.h" + +using namespace Eigen; + +class EKF_RPY +{ +public: + + EKF_RPY(float Ts, float mx0, float my0, float mz0); + + virtual ~EKF_RPY(); + + void reset(); + float get_est_state(uint8_t i); + void update(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float magnet_x, float magnet_y); + +private: + + Matrix <float, 3, 1> m0; + + float s1; + float c1; + float s2; + float c2; + float s3; + float c3; + + float g; + float kv; + float wm; + float Ts; + float rho; + Matrix <float, 4, 1> var_gy; + Matrix <float, 10, 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; + + void update_angles(); + void update_F(); + void update_H(); + void initialize_R(); + void initialize_Q(); + void update_Q(); + + Matrix <float, 10, 1> fxd(); + Matrix <float, 4, 1> gy(); + +}; + +#endif \ No newline at end of file