![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
EKF_RP.h
- Committer:
- pmic
- Date:
- 2019-10-18
- Revision:
- 0:a0e9705be9c4
- Child:
- 4:e50e18eac72b
File content as of revision 0:a0e9705be9c4:
#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