altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.h
- Revision:
- 0:a0e9705be9c4
- Child:
- 4:e50e18eac72b
diff -r 000000000000 -r a0e9705be9c4 EKF_RP.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EKF_RP.h Fri Oct 18 14:55:53 2019 +0000 @@ -0,0 +1,60 @@ +#ifndef EKF_RP_H_ +#define EKF_RP_H_ + +#include <mbed.h> +// #include "Eigen/Dense.h" +#include "Eigen/Core.h" +#include "Eigen/Geometry.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 update_F(); + void update_H(); + void initialize_R(); + void initialize_Q(); + void update_Q(); + + Matrix <float, 6, 1> fxd(); + Matrix <float, 2, 1> gy(); + +}; + +#endif \ No newline at end of file