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.h Source File

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_