Attitude estimation using IMU (3-DoF ver.)
Fork of ATTITUDE_ESTIMATION by
ATTITUDE_ESTIMATION.h@7:6fc812e342e6, 2016-12-27 (annotated)
- Committer:
- benson516
- Date:
- Tue Dec 27 07:43:25 2016 +0000
- Revision:
- 7:6fc812e342e6
- Parent:
- 6:c362ed165c39
- Child:
- 8:3882cb4be9d3
Build up the kernel function
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 | 7:6fc812e342e6 | 70 | // Get the estimation results in Eular angle |
benson516 | 7:6fc812e342e6 | 71 | // vector <--> Eular angle |
benson516 | 6:c362ed165c39 | 72 | void gVector_to_EulerAngle(const vector<float> &v_in); |
benson516 | 6:c362ed165c39 | 73 | //float* EulerAngle_to_gVector(float Eu_in[]); |
benson516 | 6:c362ed165c39 | 74 | |
benson516 | 7:6fc812e342e6 | 75 | // Setting parameters |
benson516 | 7:6fc812e342e6 | 76 | // Set L1, the diagonal matrix |
benson516 | 7:6fc812e342e6 | 77 | void Set_L1_diag(float alpha_in); // set diagnal element of gain matrix |
benson516 | 7:6fc812e342e6 | 78 | |
benson516 | 6:c362ed165c39 | 79 | |
benson516 | 0:8126c86bac2a | 80 | // Estimator |
benson516 | 6:c362ed165c39 | 81 | void Init(const vector<float> &y_in); // Let _x_est = y_in |
benson516 | 6:c362ed165c39 | 82 | void iterateOnce(const vector<float> &y_in, const vector<float> &omega_in); // Main alogorithm |
benson516 | 7:6fc812e342e6 | 83 | // Get the results |
benson516 | 6:c362ed165c39 | 84 | void getEstimation_realCoordinate(vector<float> &V_out); |
benson516 | 6:c362ed165c39 | 85 | float pitch_deg(void); |
benson516 | 6:c362ed165c39 | 86 | float roll_deg(void); |
benson516 | 6:c362ed165c39 | 87 | float yaw_deg(void); |
benson516 | 6:c362ed165c39 | 88 | |
benson516 | 6:c362ed165c39 | 89 | // Unit transformation |
benson516 | 6:c362ed165c39 | 90 | float pi; // pi = 3.1415926 |
benson516 | 6:c362ed165c39 | 91 | float deg2rad; // = 3.1415926/180.0; |
benson516 | 6:c362ed165c39 | 92 | float rad2deg; // = 180.0/3.1415926; |
benson516 | 7:6fc812e342e6 | 93 | float gravity; // = 9.81 m/s^2 |
benson516 | 0:8126c86bac2a | 94 | |
benson516 | 0:8126c86bac2a | 95 | private: |
benson516 | 6:c362ed165c39 | 96 | // Dimension |
benson516 | 6:c362ed165c39 | 97 | size_t n; |
benson516 | 6:c362ed165c39 | 98 | |
benson516 | 5:01e322f4158f | 99 | // Variables |
benson516 | 6:c362ed165c39 | 100 | vector<float> unit_nx; // (-x) direction [-1;0;0] |
benson516 | 6:c362ed165c39 | 101 | vector<float> unit_ny; // (-y) direction [0;-1;0] |
benson516 | 6:c362ed165c39 | 102 | vector<float> unit_nz; // (-z) direction [0;0;-1] |
benson516 | 6:c362ed165c39 | 103 | vector<float> zeros; // Zero vector [0;0;0] |
benson516 | 1:edc7ccfc5562 | 104 | // |
benson516 | 6:c362ed165c39 | 105 | size_t init_flag; // Flag for displaying initialization status |
benson516 | 1:edc7ccfc5562 | 106 | // float _omega_x[3][3]; // Skew symmetric matrix of omega |
benson516 | 6:c362ed165c39 | 107 | |
benson516 | 6:c362ed165c39 | 108 | vector<float> L1_diag; // Diagonal vector of gain matrix L1 |
benson516 | 6:c362ed165c39 | 109 | |
benson516 | 7:6fc812e342e6 | 110 | // |
benson516 | 7:6fc812e342e6 | 111 | // Input/output coordinate transformations within the different definitions between the "real" one and the "here" one |
benson516 | 7:6fc812e342e6 | 112 | void InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here); |
benson516 | 7:6fc812e342e6 | 113 | void OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here); |
benson516 | 7:6fc812e342e6 | 114 | |
benson516 | 7:6fc812e342e6 | 115 | |
benson516 | 7:6fc812e342e6 | 116 | // The kernel of the estimation process |
benson516 | 7:6fc812e342e6 | 117 | void EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_); |
benson516 | 7:6fc812e342e6 | 118 | void updateGyroBiasEst(void); |
benson516 | 0:8126c86bac2a | 119 | |
benson516 | 7:6fc812e342e6 | 120 | // Utilities |
benson516 | 7:6fc812e342e6 | 121 | // vector operation |
benson516 | 7:6fc812e342e6 | 122 | //float* Vector_to_SkewSymmetry(float v_in[]); |
benson516 | 7:6fc812e342e6 | 123 | //float* SkewSymmetry_to_Vector(float M_in[3][]); |
benson516 | 7:6fc812e342e6 | 124 | void Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b); // v_a X v_b |
benson516 | 7:6fc812e342e6 | 125 | vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b |
benson516 | 7:6fc812e342e6 | 126 | vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a |
benson516 | 7:6fc812e342e6 | 127 | float Get_Vector3Norm(const vector<float> &v_in); |
benson516 | 7:6fc812e342e6 | 128 | void Normolization(vector<float> &V_out, const vector<float> &V_in); |
benson516 | 7:6fc812e342e6 | 129 | |
benson516 | 7:6fc812e342e6 | 130 | // Low-pass filter, vector version |
benson516 | 6:c362ed165c39 | 131 | LPF_vector lpfv_ys; |
benson516 | 6:c362ed165c39 | 132 | LPF_vector lpfv_w; |
benson516 | 0:8126c86bac2a | 133 | }; |
benson516 | 6:c362ed165c39 | 134 | |
benson516 | 6:c362ed165c39 | 135 | #endif // _ATTITUDE_ESTIMATION_H_ |