Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers ATTITUDE_ESTIMATION.cpp Source File

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 }