AHRS
Dependencies: Eigen
Diff: EKF_RP.h
- Revision:
- 21:31e01d3e0143
- Child:
- 22:495a419e474c
diff -r 1182bc29c195 -r 31e01d3e0143 EKF_RP.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EKF_RP.h Mon Oct 21 17:14:27 2019 +0000 @@ -0,0 +1,58 @@ +#ifndef EKF_RP_H_ +#define EKF_RP_H_ + +#include <mbed.h> +#include "Eigen/Dense.h" + +using namespace Eigen; + +class EKF_RP +{ +public: + + EKF_RP(float Ts); + + virtual ~EKF_RP(); + + void reset(); + float get_est_state(uint8_t i); + void update(float gyro_x, float gyro_y, float accel_x, float accel_y); + +private: + + float s1; + float c1; + float s2; + float c2; + + float g; + float kv; + float Ts; + float rho; + Matrix <float, 2, 1> var_gy; + Matrix <float, 6, 1> var_fx; + + Matrix <float, 2, 1> u; + Matrix <float, 2, 1> y; + Matrix <float, 6, 1> x; + Matrix <float, 6, 6> F; + Matrix <float, 2, 6> H; + Matrix <float, 6, 6> Q; + Matrix <float, 2, 2> R; + Matrix <float, 6, 6> P; + Matrix <float, 6, 2> K; + Matrix <float, 6, 6> I; + + void update_angles(); + void calc_F(); + void calc_H(); + void initialize_R(); + void initialize_Q(); + void calc_Q(); + + Matrix <float, 6, 1> fxd(); + Matrix <float, 2, 1> gy(); + +}; + +#endif \ No newline at end of file