![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
EKF_RPY.h@7:bcbcc23983de, 2019-10-20 (annotated)
- Committer:
- pmic
- Date:
- Sun Oct 20 13:16:13 2019 +0000
- Revision:
- 7:bcbcc23983de
- Parent:
- 0:a0e9705be9c4
- Child:
- 12:180e09c4ea26
EKF_RPY (8 states) first time succesfully running.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 0:a0e9705be9c4 | 1 | #ifndef EKF_RPY_H_ |
pmic | 0:a0e9705be9c4 | 2 | #define EKF_RPY_H_ |
pmic | 0:a0e9705be9c4 | 3 | |
pmic | 0:a0e9705be9c4 | 4 | #include <mbed.h> |
pmic | 0:a0e9705be9c4 | 5 | #include "Eigen/Dense.h" |
pmic | 0:a0e9705be9c4 | 6 | |
pmic | 0:a0e9705be9c4 | 7 | using namespace Eigen; |
pmic | 0:a0e9705be9c4 | 8 | |
pmic | 0:a0e9705be9c4 | 9 | class EKF_RPY |
pmic | 0:a0e9705be9c4 | 10 | { |
pmic | 0:a0e9705be9c4 | 11 | public: |
pmic | 0:a0e9705be9c4 | 12 | |
pmic | 0:a0e9705be9c4 | 13 | EKF_RPY(float Ts, float mx0, float my0, float mz0); |
pmic | 0:a0e9705be9c4 | 14 | |
pmic | 0:a0e9705be9c4 | 15 | virtual ~EKF_RPY(); |
pmic | 0:a0e9705be9c4 | 16 | |
pmic | 0:a0e9705be9c4 | 17 | void reset(); |
pmic | 0:a0e9705be9c4 | 18 | float get_est_state(uint8_t i); |
pmic | 0:a0e9705be9c4 | 19 | void update(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float magnet_x, float magnet_y); |
pmic | 0:a0e9705be9c4 | 20 | |
pmic | 0:a0e9705be9c4 | 21 | private: |
pmic | 0:a0e9705be9c4 | 22 | |
pmic | 0:a0e9705be9c4 | 23 | Matrix <float, 3, 1> m0; |
pmic | 0:a0e9705be9c4 | 24 | |
pmic | 0:a0e9705be9c4 | 25 | float s1; |
pmic | 0:a0e9705be9c4 | 26 | float c1; |
pmic | 0:a0e9705be9c4 | 27 | float s2; |
pmic | 0:a0e9705be9c4 | 28 | float c2; |
pmic | 0:a0e9705be9c4 | 29 | float s3; |
pmic | 0:a0e9705be9c4 | 30 | float c3; |
pmic | 0:a0e9705be9c4 | 31 | |
pmic | 0:a0e9705be9c4 | 32 | float g; |
pmic | 0:a0e9705be9c4 | 33 | float kv; |
pmic | 0:a0e9705be9c4 | 34 | float Ts; |
pmic | 0:a0e9705be9c4 | 35 | float rho; |
pmic | 7:bcbcc23983de | 36 | Matrix <float, 4, 1> var_gy; |
pmic | 7:bcbcc23983de | 37 | Matrix <float, 8, 1> var_fx; |
pmic | 0:a0e9705be9c4 | 38 | |
pmic | 7:bcbcc23983de | 39 | Matrix <float, 3, 1> u; |
pmic | 7:bcbcc23983de | 40 | Matrix <float, 4, 1> y; |
pmic | 7:bcbcc23983de | 41 | Matrix <float, 8, 1> x; |
pmic | 7:bcbcc23983de | 42 | Matrix <float, 8, 8> F; |
pmic | 7:bcbcc23983de | 43 | Matrix <float, 4, 8> H; |
pmic | 7:bcbcc23983de | 44 | Matrix <float, 8, 8> Q; |
pmic | 7:bcbcc23983de | 45 | Matrix <float, 4, 4> R; |
pmic | 7:bcbcc23983de | 46 | Matrix <float, 8, 8> P; |
pmic | 7:bcbcc23983de | 47 | Matrix <float, 8, 4> K; |
pmic | 7:bcbcc23983de | 48 | Matrix <float, 8, 8> I; |
pmic | 0:a0e9705be9c4 | 49 | |
pmic | 0:a0e9705be9c4 | 50 | void update_angles(); |
pmic | 7:bcbcc23983de | 51 | void calc_F(); |
pmic | 7:bcbcc23983de | 52 | void calc_H(); |
pmic | 0:a0e9705be9c4 | 53 | void initialize_R(); |
pmic | 0:a0e9705be9c4 | 54 | void initialize_Q(); |
pmic | 7:bcbcc23983de | 55 | void calc_Q(); |
pmic | 0:a0e9705be9c4 | 56 | |
pmic | 7:bcbcc23983de | 57 | Matrix <float, 8, 1> fxd(); |
pmic | 7:bcbcc23983de | 58 | Matrix <float, 4, 1> gy(); |
pmic | 0:a0e9705be9c4 | 59 | |
pmic | 0:a0e9705be9c4 | 60 | }; |
pmic | 0:a0e9705be9c4 | 61 | |
pmic | 0:a0e9705be9c4 | 62 | #endif |