AHRS
Dependencies: Eigen
EKF.h@21:31e01d3e0143, 2019-10-21 (annotated)
- Committer:
- altb2
- Date:
- Mon Oct 21 17:14:27 2019 +0000
- Revision:
- 21:31e01d3e0143
- Parent:
- 20:1182bc29c195
Added EKF_RP, EKF_RPY from pmic, also EIgen Library
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 4:3c21fb0c9e84 | 1 | |
altb2 | 4:3c21fb0c9e84 | 2 | #ifndef EKF_H_ |
altb2 | 4:3c21fb0c9e84 | 3 | #define EKF_H_ |
altb2 | 4:3c21fb0c9e84 | 4 | |
altb2 | 4:3c21fb0c9e84 | 5 | #include <mbed.h> |
altb2 | 4:3c21fb0c9e84 | 6 | #include "matrix.h" |
altb2 | 4:3c21fb0c9e84 | 7 | |
altb2 | 20:1182bc29c195 | 8 | class EKF |
altb2 | 4:3c21fb0c9e84 | 9 | { |
altb2 | 4:3c21fb0c9e84 | 10 | public: |
altb2 | 4:3c21fb0c9e84 | 11 | |
altb2 | 20:1182bc29c195 | 12 | EKF(float); |
altb2 | 20:1182bc29c195 | 13 | virtual ~EKF(); |
altb2 | 4:3c21fb0c9e84 | 14 | float get_est_state(uint8_t); |
altb2 | 4:3c21fb0c9e84 | 15 | void loop(matrix *); |
altb2 | 4:3c21fb0c9e84 | 16 | void display_matrix(char); |
altb2 | 4:3c21fb0c9e84 | 17 | float getYaw(void){ |
altb2 | 4:3c21fb0c9e84 | 18 | return 0; |
altb2 | 4:3c21fb0c9e84 | 19 | } |
altb2 | 20:1182bc29c195 | 20 | void update(float,float,float,float,float,float); |
altb2 | 4:3c21fb0c9e84 | 21 | private: |
altb2 | 4:3c21fb0c9e84 | 22 | matrix x; |
altb2 | 4:3c21fb0c9e84 | 23 | matrix x_km; |
altb2 | 4:3c21fb0c9e84 | 24 | matrix P; |
altb2 | 4:3c21fb0c9e84 | 25 | matrix Q; |
altb2 | 4:3c21fb0c9e84 | 26 | matrix R; |
altb2 | 4:3c21fb0c9e84 | 27 | matrix F; |
altb2 | 4:3c21fb0c9e84 | 28 | matrix H; |
altb2 | 4:3c21fb0c9e84 | 29 | matrix K; |
altb2 | 4:3c21fb0c9e84 | 30 | matrix E; |
altb2 | 4:3c21fb0c9e84 | 31 | float g,tau_g,m,itau_g,k1,kdm; |
altb2 | 4:3c21fb0c9e84 | 32 | float Ts; |
altb2 | 4:3c21fb0c9e84 | 33 | void dgl(matrix *); |
altb2 | 20:1182bc29c195 | 34 | float my_sin(float x){return x;} |
altb2 | 21:31e01d3e0143 | 35 | float my_cos(float x){return 1.0f-0.5f*x*x;} |
altb2 | 4:3c21fb0c9e84 | 36 | }; |
altb2 | 4:3c21fb0c9e84 | 37 | |
altb2 | 4:3c21fb0c9e84 | 38 | #endif |