Attitude estimation using IMU (3-DoF ver.)
Fork of ATTITUDE_ESTIMATION by
ATTITUDE_ESTIMATION.h
00001 #ifndef _ATTITUDE_ESTIMATION_H_ 00002 #define _ATTITUDE_ESTIMATION_H_ 00003 00004 #include "mbed.h" 00005 #include "FILTER_LIB.h" 00006 #include <vector> 00007 00008 using std::vector; 00009 00010 /* 00011 class LPF_vector 00012 {public: 00013 vector<float> output; 00014 00015 LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz" 00016 vector<float> filter(const vector<float> &v_in); 00017 void reset(const vector<float> &v_in); 00018 00019 private: 00020 size_t n; 00021 float Ts; 00022 float cutOff_freq_Hz; // Hz 00023 float alpha_Ts; 00024 float One_alpha_Ts; 00025 00026 // Flag 00027 bool Flag_Init; 00028 00029 // 00030 vector<float> zeros; // Zero vector [0;0;0] 00031 }; 00032 */ 00033 00034 // Class 00035 class ATTITUDE{ 00036 public: 00037 00038 // Variables 00039 float alpha; // Convergent rate, rad/sec. 00040 float one_over_gamma; // 1/gamma, one_over_gamma == 0 means no estimation on gyro bias 00041 float Ts; // Sampling time, sec. 00042 bool enable_biasEst; // Enable the gyro-bias estimation capability 00043 bool enable_magEst; // Enable the estimation of magenetic field 00044 00045 // The map from "real" coordinate to "here" coordinate 00046 // eg. accMap_real2here = [3,-1,-2]; 00047 // means: real -> here 00048 // 1 x z 3 00049 // 2 y -x -1 00050 // 3 z -y -2 00051 vector<int> accMap_real2here; 00052 vector<int> magMap_real2here; 00053 vector<int> gyroMap_real2here; 00054 00055 vector<float> xg_est; // Estimated g-vector 00056 vector<float> xm_est; // Estimated m-vector (magnetic field) 00057 // vector<float> x_est; // Estimated state 00058 vector<float> gyroBias_est; // The estimated gyro bias in each channel 00059 vector<float> omega; // Rotation speed in body-fixed frame 00060 // 00061 vector<float> y_acce; // Accelerometer outputs 00062 vector<float> y_mag; // Magnetometer outputs 00063 00064 // Linear acceleration estimation 00065 vector<float> acce_sensorFrame; // The residual of acceleration, acce_sensorFrame = y_acce - xg_est 00066 00067 // No use, may be deleted 00068 // vector<float> w_cross_ys; // omega X ys 00069 vector<float> ys_cross_x_ys; // ys X (x_est - ys) 00070 00071 // Eular angles, in rad/s 00072 float pitch; 00073 float roll; 00074 float yaw; 00075 // 00076 float pitchRate; 00077 float rollRate; 00078 float yawRate; 00079 00080 00081 // Constructor: 00082 // Initialize the estimator 00083 ATTITUDE(float alpha_in, float Ts_in); // alpha in rad/sec., Ts in sec. 00084 00085 // Methods 00086 // Get the estimation results in Eular angle 00087 // vector <--> Eular angle 00088 void Vectors_to_EulerAngle(const vector<float> &vg_in, const vector<float> &vm_in); 00089 //float* EulerAngle_to_gVector(float Eu_in[]); 00090 00091 // Setting parameters 00092 // Set L1, the diagonal matrix 00093 void Set_L1_diag(float alpha_in); // set diagnal element of gain matrix 00094 void enable_gyroBiasEst(float gamma_in); // Enable the gyro-bias estimation 00095 00096 // Estimator 00097 void Init(void); // Let x_est = ys 00098 // Two version of estimator, with/without magenetic field estimation 00099 void iterateOnce(const vector<float> &y_acce_in, const vector<float> &omega_in); // Main alogorithm 00100 void iterateOnce(const vector<float> &y_acce_in, const vector<float> &y_mag_in, const vector<float> &omega_in); // Main alogorithm 00101 // Get the results 00102 void getEstimation_realCoordinate(vector<float> &V_out); 00103 float pitch_deg(void); 00104 float roll_deg(void); 00105 float yaw_deg(void); 00106 00107 // Unit transformation 00108 float pi; // pi = 3.1415926 00109 float deg2rad; // = 3.1415926/180.0; 00110 float rad2deg; // = 180.0/3.1415926; 00111 float gravity; // = 9.81 m/s^2 00112 00113 private: 00114 // Dimension 00115 size_t n; 00116 00117 // Variables 00118 vector<float> unit_nx; // (-x) direction [-1;0;0] 00119 vector<float> unit_ny; // (-y) direction [0;-1;0] 00120 vector<float> unit_nz; // (-z) direction [0;0;-1] 00121 vector<float> zeros; // Zero vector [0;0;0] 00122 // 00123 size_t init_flag; // Flag for displaying initialization status 00124 // float _omega_x[3][3]; // Skew symmetric matrix of omega 00125 00126 vector<float> L1_diag; // Diagonal vector of gain matrix L1 00127 00128 // 00129 // Input/output coordinate transformations within the different definitions between the "real" one and the "here" one 00130 void InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here); 00131 void OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here); 00132 00133 00134 // The kernel of the estimation process 00135 void EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_); 00136 void updateGyroBiasEst(void); 00137 00138 // Utilities 00139 // vector operation 00140 //float* Vector_to_SkewSymmetry(float v_in[]); 00141 //float* SkewSymmetry_to_Vector(float M_in[3][]); 00142 void Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b); // v_a X v_b 00143 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b 00144 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a 00145 float Get_Vector3Norm(const vector<float> &v_in); 00146 void Normolization(vector<float> &V_out, const vector<float> &V_in); 00147 00148 // Low-pass filter, vector version 00149 LPF_vector lpfv_y_acce; 00150 LPF_vector lpfv_y_mag; 00151 LPF_vector lpfv_w; 00152 }; 00153 00154 #endif // _ATTITUDE_ESTIMATION_H_
Generated on Mon Jul 18 2022 01:29:11 by
1.7.2
