Chun Feng Huang
Attitude estimation using IMU (3-DoF ver.)
Fork of
ATTITUDE_ESTIMATION
by 
LDSC_Robotics_TAs
ATTITUDE_ESTIMATION.h@0:8126c86bac2a, 2016-04-28 (annotated)
- Committer:
- benson516
- Date:
- Thu Apr 28 09:38:58 2016 +0000
- Revision:
- 0:8126c86bac2a
- Child:
- 1:edc7ccfc5562
Structure builded
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| benson516 | 0:8126c86bac2a | 1 | #ifndef _ATTITUDE_ESTIMATION_H_ |
| benson516 | 0:8126c86bac2a | 2 | #define _ATTITUDE_ESTIMATION_H_ |
| benson516 | 0:8126c86bac2a | 3 | #include "mbed.h" |
| benson516 | 0:8126c86bac2a | 4 | |
| benson516 | 0:8126c86bac2a | 5 | typedef struct{ |
| benson516 | 0:8126c86bac2a | 6 | float e[3]; |
| benson516 | 0:8126c86bac2a | 7 | } VECTOR3; |
| benson516 | 0:8126c86bac2a | 8 | typedef struct{ |
| benson516 | 0:8126c86bac2a | 9 | float e[3][3]; |
| benson516 | 0:8126c86bac2a | 10 | } MATRIX33; |
| benson516 | 0:8126c86bac2a | 11 | |
| benson516 | 0:8126c86bac2a | 12 | // Class |
| benson516 | 0:8126c86bac2a | 13 | class ATTITUDE{ |
| benson516 | 0:8126c86bac2a | 14 | public: |
| benson516 | 0:8126c86bac2a | 15 | |
| benson516 | 0:8126c86bac2a | 16 | // Variables |
| benson516 | 0:8126c86bac2a | 17 | VECTOR3 x_est; |
| benson516 | 0:8126c86bac2a | 18 | VECTOR3 EulerAngle; |
| benson516 | 0:8126c86bac2a | 19 | |
| benson516 | 0:8126c86bac2a | 20 | |
| benson516 | 0:8126c86bac2a | 21 | // Constructor: |
| benson516 | 0:8126c86bac2a | 22 | // Initialize the estimator, |
| benson516 | 0:8126c86bac2a | 23 | ATTITUDE(); |
| benson516 | 0:8126c86bac2a | 24 | |
| benson516 | 0:8126c86bac2a | 25 | // Methods |
| benson516 | 0:8126c86bac2a | 26 | // Transformations |
| benson516 | 0:8126c86bac2a | 27 | MATRIX33 Vector_to_SkewSymmetry(VECTOR3); |
| benson516 | 0:8126c86bac2a | 28 | VECTOR3 SkewSymmetry_to_Vector(MATRIX33); |
| benson516 | 0:8126c86bac2a | 29 | VECTOR3 gVector_to_EulerAngle(VECTOR3); |
| benson516 | 0:8126c86bac2a | 30 | VECTOR3 EulerAngle_to_gVector(VECTOR3); |
| benson516 | 0:8126c86bac2a | 31 | // Estimator |
| benson516 | 0:8126c86bac2a | 32 | void Init_Estimator(VECTOR3); |
| benson516 | 0:8126c86bac2a | 33 | void Reset_Estimator(VECTOR3); |
| benson516 | 0:8126c86bac2a | 34 | void Run_Estimator(VECTOR3); |
| benson516 | 0:8126c86bac2a | 35 | |
| benson516 | 0:8126c86bac2a | 36 | private: |
| benson516 | 0:8126c86bac2a | 37 | int8_t _init_flag; // Flag for displaying initialization status |
| benson516 | 0:8126c86bac2a | 38 | VECTOR3 _x_est; // Estimated state |
| benson516 | 0:8126c86bac2a | 39 | VECTOR3 _y; // Sensor output |
| benson516 | 0:8126c86bac2a | 40 | VECTOR3 _omega; // Rotation speed in body-fixed frame |
| benson516 | 0:8126c86bac2a | 41 | // MATRIX33 _Omega_cross; // Skew-symetric matrix of rotaion speed |
| benson516 | 0:8126c86bac2a | 42 | |
| benson516 | 0:8126c86bac2a | 43 | MATRIX33 _L1; // Gain matrix |
| benson516 | 0:8126c86bac2a | 44 | float _Ts; // Sampling time |
| benson516 | 0:8126c86bac2a | 45 | |
| benson516 | 0:8126c86bac2a | 46 | // Method |
| benson516 | 0:8126c86bac2a | 47 | void Set_x_est(VECTOR3); |
| benson516 | 0:8126c86bac2a | 48 | void Set_measurements(float[],float[]); // array -> VECTOR33; y, omega |
| benson516 | 0:8126c86bac2a | 49 | void Set_L1(MATRIX33); // set gain matrix |
| benson516 | 0:8126c86bac2a | 50 | |
| benson516 | 0:8126c86bac2a | 51 | }; |
| benson516 | 0:8126c86bac2a | 52 | #endif // _ATTITUDE_ESTIMATION_H_ |