AHRS
Dependencies: Eigen
EKF_RPY.h@30:9b0cd3caf0ec, 2020-01-27 (annotated)
- Committer:
- pmic
- Date:
- Mon Jan 27 10:54:13 2020 +0000
- Revision:
- 30:9b0cd3caf0ec
- Parent:
- 23:71996bfe68eb
Correct magnetometer.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 21:31e01d3e0143 | 1 | #ifndef EKF_RPY_H_ |
altb2 | 21:31e01d3e0143 | 2 | #define EKF_RPY_H_ |
altb2 | 21:31e01d3e0143 | 3 | |
altb2 | 21:31e01d3e0143 | 4 | #include <mbed.h> |
altb2 | 21:31e01d3e0143 | 5 | #include "Eigen/Dense.h" |
altb2 | 21:31e01d3e0143 | 6 | |
altb2 | 21:31e01d3e0143 | 7 | using namespace Eigen; |
altb2 | 21:31e01d3e0143 | 8 | |
altb2 | 21:31e01d3e0143 | 9 | class EKF_RPY |
altb2 | 21:31e01d3e0143 | 10 | { |
altb2 | 21:31e01d3e0143 | 11 | public: |
altb2 | 21:31e01d3e0143 | 12 | |
altb2 | 21:31e01d3e0143 | 13 | EKF_RPY(float Ts, float mx0, float my0, float mz0); |
altb2 | 21:31e01d3e0143 | 14 | EKF_RPY(float Ts); |
altb2 | 21:31e01d3e0143 | 15 | |
altb2 | 21:31e01d3e0143 | 16 | virtual ~EKF_RPY(); |
altb2 | 21:31e01d3e0143 | 17 | |
altb2 | 22:495a419e474c | 18 | void set_para(); |
altb2 | 21:31e01d3e0143 | 19 | void reset(); |
altb2 | 23:71996bfe68eb | 20 | void increase_diag_P(); |
altb2 | 22:495a419e474c | 21 | void set_m0(float mx0, float my0, float mz0); |
altb2 | 21:31e01d3e0143 | 22 | float get_est_state(uint8_t i); |
altb2 | 21:31e01d3e0143 | 23 | void update(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float magnet_x, float magnet_y); |
altb2 | 21:31e01d3e0143 | 24 | |
altb2 | 21:31e01d3e0143 | 25 | private: |
altb2 | 21:31e01d3e0143 | 26 | |
altb2 | 21:31e01d3e0143 | 27 | Matrix <float, 3, 1> m0; |
altb2 | 21:31e01d3e0143 | 28 | |
altb2 | 21:31e01d3e0143 | 29 | float s1; |
altb2 | 21:31e01d3e0143 | 30 | float c1; |
altb2 | 21:31e01d3e0143 | 31 | float s2; |
altb2 | 21:31e01d3e0143 | 32 | float c2; |
altb2 | 21:31e01d3e0143 | 33 | float s3; |
altb2 | 21:31e01d3e0143 | 34 | float c3; |
altb2 | 21:31e01d3e0143 | 35 | |
altb2 | 23:71996bfe68eb | 36 | float scale_P0; |
altb2 | 21:31e01d3e0143 | 37 | float g; |
altb2 | 21:31e01d3e0143 | 38 | float kv; |
altb2 | 21:31e01d3e0143 | 39 | float Ts; |
altb2 | 21:31e01d3e0143 | 40 | float rho; |
altb2 | 21:31e01d3e0143 | 41 | Matrix <float, 4, 1> var_gy; |
altb2 | 21:31e01d3e0143 | 42 | Matrix <float, 8, 1> var_fx; |
altb2 | 21:31e01d3e0143 | 43 | |
altb2 | 21:31e01d3e0143 | 44 | Matrix <float, 3, 1> u; |
altb2 | 21:31e01d3e0143 | 45 | Matrix <float, 4, 1> y; |
altb2 | 21:31e01d3e0143 | 46 | Matrix <float, 8, 1> x; |
altb2 | 21:31e01d3e0143 | 47 | Matrix <float, 8, 8> F; |
altb2 | 21:31e01d3e0143 | 48 | Matrix <float, 4, 8> H; |
altb2 | 21:31e01d3e0143 | 49 | Matrix <float, 8, 8> Q; |
altb2 | 21:31e01d3e0143 | 50 | Matrix <float, 4, 4> R; |
altb2 | 21:31e01d3e0143 | 51 | Matrix <float, 8, 8> P; |
altb2 | 21:31e01d3e0143 | 52 | Matrix <float, 8, 4> K; |
altb2 | 21:31e01d3e0143 | 53 | Matrix <float, 8, 8> I; |
altb2 | 22:495a419e474c | 54 | Matrix <float, 4, 1> e; |
altb2 | 21:31e01d3e0143 | 55 | |
altb2 | 21:31e01d3e0143 | 56 | void update_angles(); |
altb2 | 21:31e01d3e0143 | 57 | void calc_F(); |
altb2 | 21:31e01d3e0143 | 58 | void calc_H(); |
altb2 | 21:31e01d3e0143 | 59 | void initialize_R(); |
altb2 | 21:31e01d3e0143 | 60 | void initialize_Q(); |
altb2 | 21:31e01d3e0143 | 61 | void calc_Q(); |
altb2 | 21:31e01d3e0143 | 62 | |
altb2 | 21:31e01d3e0143 | 63 | Matrix <float, 8, 1> fxd(); |
altb2 | 21:31e01d3e0143 | 64 | Matrix <float, 4, 1> gy(); |
altb2 | 21:31e01d3e0143 | 65 | |
altb2 | 21:31e01d3e0143 | 66 | }; |
altb2 | 21:31e01d3e0143 | 67 | |
altb2 | 21:31e01d3e0143 | 68 | #endif |