Attitude estimation using IMU (3-DoF ver.)
Fork of ATTITUDE_ESTIMATION by
ATTITUDE_ESTIMATION.cpp
00001 #include "ATTITUDE_ESTIMATION.h" 00002 00003 /* 00004 //=====================LPF ====================// 00005 LPF_vector::LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in) 00006 { 00007 n = dimension; 00008 Ts = samplingTime; 00009 cutOff_freq_Hz = cutOff_freq_Hz_in; 00010 alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts; 00011 One_alpha_Ts = 1.0 - alpha_Ts; 00012 00013 zeros.assign(n, 0.0); 00014 00015 output = zeros; 00016 00017 // 00018 Flag_Init = false; 00019 } 00020 00021 vector<float> LPF_vector::filter(const vector<float> &input) 00022 { 00023 // Initialization 00024 if (!Flag_Init){ 00025 reset(input); 00026 Flag_Init = true; 00027 return output; 00028 } 00029 00030 for (size_t i = 0; i < n; ++i){ 00031 // output = One_alpha_Ts*output + alpha_Ts*input; 00032 output[i] += alpha_Ts*(input[i] - output[i]); 00033 } 00034 00035 return output; 00036 } 00037 void LPF_vector::reset(const vector<float> &input) 00038 { 00039 // output = (1.0 - alpha_Ts)*output + alpha_Ts*input; 00040 output = input; 00041 return; 00042 } 00043 */ 00044 00045 //-------------------------------------------------------// 00046 ATTITUDE::ATTITUDE(float alpha_in, float Ts_in): 00047 alpha(alpha_in), 00048 Ts(Ts_in), 00049 lpfv_y_acce(3, Ts_in, 200.0), // Input filter for accelerometers 00050 lpfv_y_mag(3, Ts_in, 200.0), // Input filter for magenetometers 00051 lpfv_w(3, Ts_in, 200.0) // Input filter for gyroscope 00052 { 00053 // Dimension 00054 n = 3; 00055 // 00056 init_flag = 0; // Uninitialized 00057 00058 // Default: close the gyro-bias estimation 00059 enable_biasEst = false; 00060 one_over_gamma = 0.0; 00061 // Default: close the estimation for magenetic field 00062 enable_magEst = false; 00063 00064 // Unit transformation 00065 pi = 3.1415926; 00066 deg2rad = pi/180.0; 00067 rad2deg = 180.0/pi; 00068 gravity = 9.81; // m/s^2 00069 00070 // The map from "real" coordinate to "here" coordinate 00071 // eg. accMap_real2here = [3,-1,-2]; 00072 // means: real -> here 00073 // 1 x z 3 00074 // 2 y -x -1 00075 // 3 z -y -2 00076 // int accmap_temp[] = {3,-1,-2}; 00077 int accmap_temp[] = {-3,1,2}; // Reverse: The direction of accelerometer is defined based on the direction of the acceleration of the sensor, not the g-direction 00078 // int accmap_temp[] = {1, 2, 3}; 00079 // 00080 int magMap_temp[] = {3,-1, 2}; // real z-axis is in the reverse direction 00081 int gyroMap_temp[] = {3,-1,-2}; 00082 accMap_real2here.assign(accmap_temp, accmap_temp+n); 00083 magMap_real2here.assign(magMap_temp, magMap_temp+n); 00084 gyroMap_real2here.assign(gyroMap_temp, gyroMap_temp+n); 00085 00086 // zeros 00087 zeros.assign(n,0.0); 00088 // unit_nz 00089 unit_nx = zeros; 00090 unit_ny = zeros; 00091 unit_nz = zeros; 00092 unit_nx[0] = -1; // negative x 00093 unit_ny[1] = -1; // negative y 00094 unit_nz[2] = -1; // negative z 00095 00096 // States 00097 xg_est = unit_nx; // g is pointing downward 00098 xm_est = Get_VectorScalarMultiply(unit_nz, -1.0); // m is pointing forward 00099 gyroBias_est = zeros; 00100 omega = zeros; 00101 // 00102 y_acce = zeros; // Accelerometer outputs 00103 y_mag = zeros; // Magnetometer outputs 00104 00105 // Linear acceleration estimation 00106 acce_sensorFrame = zeros; 00107 // 00108 // w_cross_ys = zeros; // omega X ys 00109 ys_cross_x_ys = zeros; // ys X (x_est - ys) 00110 00111 // Eular angles, in rad/s 00112 pitch = 0.0; 00113 roll = 0.0; 00114 yaw = 0.0; 00115 // 00116 pitchRate = 0.0; 00117 rollRate = 0.0; 00118 yawRate = 0.0; 00119 00120 // Gain matrix 00121 Set_L1_diag(alpha); 00122 00123 } 00124 // Public methods 00125 void ATTITUDE::Vectors_to_EulerAngle(const vector<float> &vg_in, const vector<float> &vm_in){ 00126 // 00127 // This function should be customized according to the definition of coordinate system 00128 // 00129 00130 /* 00131 // Here we follow the definition in bicycle paper 00132 yaw = 0.0; // phi, yaw 00133 roll = atan2(-vg_in[1],vg_in[0]); // theta, roll 00134 pitch = atan2(cos(roll)*vg_in[2],vg_in[0]); // psi, pitch 00135 */ 00136 00137 // Eular angle: 1-3-2, zs is pointing forward 00138 // yaw = 0.0; // phi, yaw 00139 pitch = atan2(-vg_in[2],-vg_in[0]); // psi, pitch 00140 if (abs(pitch) < 0.7854){ // pi/4 00141 roll = atan2(cos(pitch)*vg_in[1],-vg_in[0]); // theta, roll 00142 }else{ 00143 if (pitch >= 0.0) 00144 roll = atan2(sin(pitch)*vg_in[1],-vg_in[2]); // theta, roll 00145 else 00146 roll = atan2(-sin(pitch)*vg_in[1],vg_in[2]); // theta, roll 00147 } 00148 00149 // Calculate the yaw angle 00150 if (enable_magEst){ 00151 float num = vm_in[1]*cos(pitch); 00152 float den = (vm_in[2]*cos(roll) - vm_in[1]*sin(pitch)*sin(roll)); 00153 yaw = atan2(num, den); 00154 }else{ 00155 yaw = 0.0; // phi, yaw 00156 } 00157 00158 00159 // Calculate the rate of each Eular angle 00160 if (abs(roll) > 1.2217 ){ // 75 deg, singular case 00161 yawRate = 0.0; // 0.0, approximation 00162 pitchRate = omega[1]; // w2, approximation 00163 }else{ // Normal case 00164 // yawRate = (w1*cos(psi) + w3*sin(psi))/cos(theta) 00165 yawRate = (omega[0]*cos(pitch) + omega[2]*sin(pitch))/cos(roll); 00166 // pitchRate = w2 + yawRate*sin(theta) 00167 pitchRate = omega[1] + yawRate*sin(roll); 00168 } 00169 // rollRate = w3*cos(psi) - w1*sin(psi) 00170 rollRate = (omega[2]*cos(pitch) - omega[0]*sin(pitch)); 00171 00172 } 00173 // Setting parameters 00174 // Set L1, the diagonal matrix 00175 void ATTITUDE::Set_L1_diag(float alpha_in) // set diagnal element of gain matrix 00176 { 00177 alpha = alpha_in; 00178 L1_diag.assign(n,alpha_in); 00179 } 00180 void ATTITUDE::enable_gyroBiasEst(float gamma_in){ // Enable the gyro-bias estimation 00181 enable_biasEst = true; 00182 one_over_gamma = 1/gamma_in; 00183 } 00184 // 00185 void ATTITUDE::Init(void) // Let x_est = ys 00186 { 00187 // y_acce = y_in; 00188 // Normolization(xg_est,y_in); // xg_est be set as normalized y_in 00189 xg_est = y_acce; 00190 xm_est = y_mag; 00191 ++init_flag; 00192 } 00193 void ATTITUDE::iterateOnce(const vector<float> &y_acce_in, const vector<float> &omega_in) // Main alogorithm 00194 { 00195 enable_magEst = false; // no magenetometers input 00196 // Input mapping 00197 InputMapping(y_acce, y_acce_in, accMap_real2here); 00198 // InputMapping(y_mag, y_mag_in, magMap_real2here); 00199 InputMapping(omega, omega_in, gyroMap_real2here); 00200 00201 // Input filter 00202 // y_acce = lpfv_y_acce.filter(y_acce); 00203 // y_mag = lpfv_y_mag.filter(y_mag); 00204 // omega = lpfv_w.filter(omega); 00205 00206 // gyro-bias estimation 00207 if (enable_biasEst){ 00208 omega = Get_VectorPlus(omega, gyroBias_est, true); // minus, omega - gyroBias_est 00209 } 00210 00211 // 00212 if(init_flag < 3){ 00213 Init(); 00214 } 00215 else{ 00216 // Estimation kernel process 00217 EstimationKernel(xg_est, y_acce, omega); 00218 // EstimationKernel(xm_est, y_mag, omega); 00219 00220 // gyro-bias estimation 00221 if (enable_biasEst){ 00222 updateGyroBiasEst(); 00223 } 00224 } 00225 00226 // Calculate the linear acceleration 00227 acce_sensorFrame = Get_VectorPlus(y_acce, xg_est, true); // minus 00228 00229 // 00230 Vectors_to_EulerAngle(xg_est,xm_est); 00231 } 00232 void ATTITUDE::iterateOnce(const vector<float> &y_acce_in, const vector<float> &y_mag_in, const vector<float> &omega_in) // Main alogorithm 00233 { 00234 enable_magEst = true; // with magenetometers input 00235 // Input mapping 00236 InputMapping(y_acce, y_acce_in, accMap_real2here); 00237 InputMapping(y_mag, y_mag_in, magMap_real2here); 00238 InputMapping(omega, omega_in, gyroMap_real2here); 00239 00240 // Input filter 00241 // y_acce = lpfv_y_acce.filter(y_acce); 00242 // y_mag = lpfv_y_mag.filter(y_mag); 00243 // omega = lpfv_w.filter(omega); 00244 00245 // gyro-bias estimation 00246 if (enable_biasEst){ 00247 omega = Get_VectorPlus(omega, gyroBias_est, true); // minus, omega - gyroBias_est 00248 } 00249 00250 // 00251 if(init_flag < 3){ 00252 Init(); 00253 } 00254 else{ 00255 // Estimation kernel process 00256 EstimationKernel(xg_est, y_acce, omega); 00257 EstimationKernel(xm_est, y_mag, omega); 00258 00259 // gyro-bias estimation 00260 if (enable_biasEst){ 00261 updateGyroBiasEst(); 00262 } 00263 } 00264 00265 // Calculate the linear acceleration 00266 acce_sensorFrame = Get_VectorPlus(y_acce, xg_est, true); // minus 00267 // 00268 Vectors_to_EulerAngle(xg_est,xm_est); 00269 } 00270 // transform the x_est into "real" coordinate 00271 void ATTITUDE::getEstimation_realCoordinate(vector<float> &V_out){ 00272 OutputMapping(V_out,xg_est,accMap_real2here); 00273 } 00274 // Get Eular angles 00275 float ATTITUDE::pitch_deg(void){ 00276 return (rad2deg*pitch); 00277 } 00278 float ATTITUDE::roll_deg(void){ 00279 return (rad2deg*roll); 00280 } 00281 float ATTITUDE::yaw_deg(void){ 00282 return (rad2deg*yaw); 00283 } 00284 00285 // Private methods 00286 //////////////////////////////////// 00287 // Input/output coordinate transformations within the different definitions between the "real" one and the "here" one 00288 // real -> here 00289 void ATTITUDE::InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here){ 00290 // The map from "real" coordinate to "here" coordinate 00291 // eg. accMap_real2here = [3,-1,-2]; 00292 // means: real -> here 00293 // 1 x z 3 00294 // 2 y -x -1 00295 // 3 z -y -2 00296 // vector<int> accMap_real2here = {3,-1,-2}; 00297 // vector<int> gyroMap_real2here = {3,-1,-2}; 00298 00299 // Iterate through "real" coordinates 00300 int idx_here = 1; 00301 for (size_t i = 0; i < n; ++i){ 00302 idx_here = map_real2here[i]; 00303 if (idx_here > 0){ 00304 v_hereDef[idx_here-1] = v_realDef[i]; 00305 }else{ 00306 v_hereDef[-idx_here-1] = -1*v_realDef[i]; 00307 } 00308 } 00309 } 00310 // here -> real 00311 void ATTITUDE::OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here){ 00312 // This is the inverse mapping of the InputMapping 00313 00314 // The map from "real" coordinate to "here" coordinate 00315 // eg. accMap_real2here = [3,-1,-2]; 00316 // means: real -> here 00317 // 1 x z 3 00318 // 2 y -x -1 00319 // 3 z -y -2 00320 // vector<int> accMap_real2here = {3,-1,-2}; 00321 // vector<int> gyroMap_real2here = {3,-1,-2}; 00322 00323 // Iterate through "real" coordinates 00324 int idx_here = 1; 00325 for (size_t i = 0; i < n; ++i){ 00326 idx_here = map_real2here[i]; 00327 if (idx_here > 0){ 00328 v_realDef[i] = v_hereDef[idx_here-1]; 00329 }else{ 00330 v_realDef[i] = -1*v_hereDef[-idx_here-1]; 00331 } 00332 } 00333 } 00334 // The kernel of the estimation process 00335 //////////////////////////////////////// 00336 void ATTITUDE::EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_){ 00337 static vector<float> _w_cross_ys_; 00338 Get_CrossProduct3(_w_cross_ys_, _omega_, _ys_); 00339 for(size_t i = 0; i < n; ++i){ 00340 // x_est_plus[i] = x_est[i] + Ts*( L1_diag[i]*(ys[i] - x_est[i]) - w_cross_ys[i]); 00341 _x_est_[i] += Ts*( L1_diag[i]*(_ys_[i] - _x_est_[i]) - _w_cross_ys_[i]); 00342 } 00343 00344 } 00345 void ATTITUDE::updateGyroBiasEst(void){ // Update the gyro bias estimation 00346 if (one_over_gamma == 0.0){ 00347 return; 00348 } 00349 // 00350 Get_CrossProduct3(ys_cross_x_ys, y_acce, Get_VectorPlus(xg_est,y_acce,true)); 00351 // 00352 gyroBias_est = Get_VectorPlus(gyroBias_est, Get_VectorScalarMultiply(ys_cross_x_ys, (one_over_gamma)), true); 00353 } 00354 00355 //////////////////////////////////////// end The kernal of the estimation process 00356 00357 // Utilities 00358 // vector operation 00359 void ATTITUDE::Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b) // v_a X v_b 00360 { 00361 // Check the size 00362 if (v_c.size() != n){ 00363 v_c.resize(n); 00364 } 00365 v_c[0] = (-v_a[2]*v_b[1]) + v_a[1]*v_b[2]; 00366 v_c[1] = v_a[2]*v_b[0] - v_a[0]*v_b[2]; 00367 v_c[2] = (-v_a[1]*v_b[0]) + v_a[0]*v_b[1]; 00368 } 00369 vector<float> ATTITUDE::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b 00370 { 00371 static vector<float> v_c(n); 00372 for (size_t i = 0; i < n; ++i){ 00373 if (is_minus){ 00374 v_c[i] = v_a[i] - v_b[i]; 00375 }else{ 00376 v_c[i] = v_a[i] + v_b[i]; 00377 } 00378 } 00379 return v_c; 00380 } 00381 vector<float> ATTITUDE::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a 00382 { 00383 static vector<float> v_c(n); 00384 for (size_t i = 0; i < n; ++i){ 00385 v_c[i] = scale*v_a[i]; 00386 00387 } 00388 return v_c; 00389 } 00390 float ATTITUDE::Get_Vector3Norm(const vector<float> &v_in) 00391 { 00392 float temp = 0.0; 00393 00394 for (size_t i = 0; i < n; ++i) 00395 temp += v_in[i]*v_in[i]; 00396 return sqrt(temp); // <- Should check if this function is available (?) 00397 } 00398 void ATTITUDE::Normolization(vector<float> &V_out, const vector<float> &V_in){ 00399 float norm = Get_Vector3Norm(V_in); 00400 for (size_t i = 0; i < n; ++i){ 00401 V_out[i] = V_in[i]/norm; 00402 } 00403 }
Generated on Mon Jul 18 2022 01:29:11 by
1.7.2
