Attitude estimation using IMU (3-DoF ver.)
Fork of ATTITUDE_ESTIMATION by
ATTITUDE_ESTIMATION.h@6:c362ed165c39, 2016-12-24 (annotated)
- Committer:
- benson516
- Date:
- Sat Dec 24 09:51:20 2016 +0000
- Revision:
- 6:c362ed165c39
- Parent:
- 5:01e322f4158f
- Child:
- 7:6fc812e342e6
Check function well
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 | 6:c362ed165c39 | 3 | |
benson516 | 0:8126c86bac2a | 4 | #include "mbed.h" |
benson516 | 6:c362ed165c39 | 5 | #include <vector> |
benson516 | 6:c362ed165c39 | 6 | |
benson516 | 6:c362ed165c39 | 7 | using std::vector; |
benson516 | 6:c362ed165c39 | 8 | |
benson516 | 6:c362ed165c39 | 9 | |
benson516 | 6:c362ed165c39 | 10 | class LPF_vector |
benson516 | 6:c362ed165c39 | 11 | {public: |
benson516 | 6:c362ed165c39 | 12 | vector<float> output; |
benson516 | 6:c362ed165c39 | 13 | |
benson516 | 6:c362ed165c39 | 14 | LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz" |
benson516 | 6:c362ed165c39 | 15 | vector<float> filter(const vector<float> &v_in); |
benson516 | 6:c362ed165c39 | 16 | void reset(const vector<float> &v_in); |
benson516 | 6:c362ed165c39 | 17 | |
benson516 | 6:c362ed165c39 | 18 | private: |
benson516 | 6:c362ed165c39 | 19 | size_t n; |
benson516 | 6:c362ed165c39 | 20 | float Ts; |
benson516 | 6:c362ed165c39 | 21 | float cutOff_freq_Hz; // Hz |
benson516 | 6:c362ed165c39 | 22 | float alpha_Ts; |
benson516 | 6:c362ed165c39 | 23 | float One_alpha_Ts; |
benson516 | 6:c362ed165c39 | 24 | |
benson516 | 6:c362ed165c39 | 25 | // Flag |
benson516 | 6:c362ed165c39 | 26 | bool Flag_Init; |
benson516 | 6:c362ed165c39 | 27 | |
benson516 | 6:c362ed165c39 | 28 | // |
benson516 | 6:c362ed165c39 | 29 | vector<float> zeros; // Zero vector [0;0;0] |
benson516 | 6:c362ed165c39 | 30 | }; |
benson516 | 6:c362ed165c39 | 31 | |
benson516 | 0:8126c86bac2a | 32 | |
benson516 | 0:8126c86bac2a | 33 | // Class |
benson516 | 0:8126c86bac2a | 34 | class ATTITUDE{ |
benson516 | 0:8126c86bac2a | 35 | public: |
benson516 | 6:c362ed165c39 | 36 | |
benson516 | 0:8126c86bac2a | 37 | // Variables |
benson516 | 1:edc7ccfc5562 | 38 | float alpha; // Convergent rate, rad/sec. |
benson516 | 6:c362ed165c39 | 39 | float one_over_gamma; // 1/gamma, one_over_gamma == 0 means no estimation on gyro bias |
benson516 | 1:edc7ccfc5562 | 40 | float Ts; // Sampling time, sec. |
benson516 | 6:c362ed165c39 | 41 | bool enable_biasEst; // Enable the gyro-bias estimation capability |
benson516 | 6:c362ed165c39 | 42 | |
benson516 | 6:c362ed165c39 | 43 | // The map from "real" coordinate to "here" coordinate |
benson516 | 6:c362ed165c39 | 44 | // eg. accMap_real2here = [3,-1,-2]; |
benson516 | 6:c362ed165c39 | 45 | // means: real -> here |
benson516 | 6:c362ed165c39 | 46 | // 1 x z 3 |
benson516 | 6:c362ed165c39 | 47 | // 2 y -x -1 |
benson516 | 6:c362ed165c39 | 48 | // 3 z -y -2 |
benson516 | 6:c362ed165c39 | 49 | vector<int> accMap_real2here; |
benson516 | 6:c362ed165c39 | 50 | vector<int> gyroMap_real2here; |
benson516 | 6:c362ed165c39 | 51 | |
benson516 | 6:c362ed165c39 | 52 | vector<float> x_est; // Estimated state |
benson516 | 6:c362ed165c39 | 53 | vector<float> gyroBias_est; // The estimated gyro bias in each channel |
benson516 | 6:c362ed165c39 | 54 | vector<float> omega; // Rotation speed in body-fixed frame |
benson516 | 6:c362ed165c39 | 55 | vector<float> ys; // Sensor output |
benson516 | 6:c362ed165c39 | 56 | vector<float> w_cross_ys; // omega X ys |
benson516 | 6:c362ed165c39 | 57 | vector<float> ys_cross_x_ys; // ys X (x_est - ys) |
benson516 | 6:c362ed165c39 | 58 | |
benson516 | 6:c362ed165c39 | 59 | // Eular angles, in rad/s |
benson516 | 6:c362ed165c39 | 60 | float pitch; |
benson516 | 6:c362ed165c39 | 61 | float roll; |
benson516 | 6:c362ed165c39 | 62 | float yaw; |
benson516 | 6:c362ed165c39 | 63 | |
benson516 | 6:c362ed165c39 | 64 | |
benson516 | 0:8126c86bac2a | 65 | // Constructor: |
benson516 | 1:edc7ccfc5562 | 66 | // Initialize the estimator |
benson516 | 6:c362ed165c39 | 67 | ATTITUDE(float alpha_in, float one_over_gamma_in, float Ts_in); // alpha in rad/sec., Ts in sec. |
benson516 | 6:c362ed165c39 | 68 | |
benson516 | 0:8126c86bac2a | 69 | // Methods |
benson516 | 0:8126c86bac2a | 70 | // Transformations |
benson516 | 6:c362ed165c39 | 71 | void InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here); |
benson516 | 6:c362ed165c39 | 72 | void OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here); |
benson516 | 1:edc7ccfc5562 | 73 | //float* Vector_to_SkewSymmetry(float v_in[]); |
benson516 | 1:edc7ccfc5562 | 74 | //float* SkewSymmetry_to_Vector(float M_in[3][]); |
benson516 | 6:c362ed165c39 | 75 | void gVector_to_EulerAngle(const vector<float> &v_in); |
benson516 | 6:c362ed165c39 | 76 | //float* EulerAngle_to_gVector(float Eu_in[]); |
benson516 | 6:c362ed165c39 | 77 | |
benson516 | 1:edc7ccfc5562 | 78 | // vector operation |
benson516 | 6:c362ed165c39 | 79 | void Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b); // v_a X v_b |
benson516 | 6:c362ed165c39 | 80 | vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b |
benson516 | 6:c362ed165c39 | 81 | vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a |
benson516 | 6:c362ed165c39 | 82 | float Get_Vector3Norm(const vector<float> &v_in); |
benson516 | 6:c362ed165c39 | 83 | void Normolization(vector<float> &V_out, const vector<float> &V_in); |
benson516 | 6:c362ed165c39 | 84 | |
benson516 | 0:8126c86bac2a | 85 | // Estimator |
benson516 | 6:c362ed165c39 | 86 | void Init(const vector<float> &y_in); // Let _x_est = y_in |
benson516 | 6:c362ed165c39 | 87 | void iterateOnce(const vector<float> &y_in, const vector<float> &omega_in); // Main alogorithm |
benson516 | 6:c362ed165c39 | 88 | void updateGyroBiasEst(void); |
benson516 | 6:c362ed165c39 | 89 | void getEstimation_realCoordinate(vector<float> &V_out); |
benson516 | 6:c362ed165c39 | 90 | float pitch_deg(void); |
benson516 | 6:c362ed165c39 | 91 | float roll_deg(void); |
benson516 | 6:c362ed165c39 | 92 | float yaw_deg(void); |
benson516 | 6:c362ed165c39 | 93 | |
benson516 | 6:c362ed165c39 | 94 | // Unit transformation |
benson516 | 6:c362ed165c39 | 95 | float pi; // pi = 3.1415926 |
benson516 | 6:c362ed165c39 | 96 | float deg2rad; // = 3.1415926/180.0; |
benson516 | 6:c362ed165c39 | 97 | float rad2deg; // = 180.0/3.1415926; |
benson516 | 0:8126c86bac2a | 98 | |
benson516 | 0:8126c86bac2a | 99 | private: |
benson516 | 6:c362ed165c39 | 100 | // Dimension |
benson516 | 6:c362ed165c39 | 101 | size_t n; |
benson516 | 6:c362ed165c39 | 102 | |
benson516 | 5:01e322f4158f | 103 | // Variables |
benson516 | 6:c362ed165c39 | 104 | vector<float> unit_nx; // (-x) direction [-1;0;0] |
benson516 | 6:c362ed165c39 | 105 | vector<float> unit_ny; // (-y) direction [0;-1;0] |
benson516 | 6:c362ed165c39 | 106 | vector<float> unit_nz; // (-z) direction [0;0;-1] |
benson516 | 6:c362ed165c39 | 107 | vector<float> zeros; // Zero vector [0;0;0] |
benson516 | 1:edc7ccfc5562 | 108 | // |
benson516 | 6:c362ed165c39 | 109 | size_t init_flag; // Flag for displaying initialization status |
benson516 | 1:edc7ccfc5562 | 110 | // float _omega_x[3][3]; // Skew symmetric matrix of omega |
benson516 | 6:c362ed165c39 | 111 | |
benson516 | 6:c362ed165c39 | 112 | vector<float> L1_diag; // Diagonal vector of gain matrix L1 |
benson516 | 6:c362ed165c39 | 113 | |
benson516 | 5:01e322f4158f | 114 | // Methods |
benson516 | 6:c362ed165c39 | 115 | void Set_L1_diag(float alpha_in); // set diagnal element of gain matrix |
benson516 | 0:8126c86bac2a | 116 | |
benson516 | 6:c362ed165c39 | 117 | LPF_vector lpfv_ys; |
benson516 | 6:c362ed165c39 | 118 | LPF_vector lpfv_w; |
benson516 | 0:8126c86bac2a | 119 | }; |
benson516 | 6:c362ed165c39 | 120 | |
benson516 | 6:c362ed165c39 | 121 | #endif // _ATTITUDE_ESTIMATION_H_ |