Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

ATTITUDE_ESTIMATION.h@11:394a59f3b1f6, 2017-01-04 (annotated)

Committer:
benson516
Date:
Wed Jan 04 14:39:26 2017 +0000
Revision:
11:394a59f3b1f6
Parent:
10:166006e89252
Child:
13:16bc19155f54
Change the cut-off frequency of the y_acce filter

Who changed what in which revision?

UserRevisionLine numberNew 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 10:166006e89252 5 #include "FILTER_LIB.h"
benson516 6:c362ed165c39 6 #include <vector>
benson516 6:c362ed165c39 7
benson516 6:c362ed165c39 8 using std::vector;
benson516 6:c362ed165c39 9
benson516 10:166006e89252 10 /*
benson516 6:c362ed165c39 11 class LPF_vector
benson516 6:c362ed165c39 12 {public:
benson516 6:c362ed165c39 13 vector<float> output;
benson516 6:c362ed165c39 14
benson516 6:c362ed165c39 15 LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
benson516 6:c362ed165c39 16 vector<float> filter(const vector<float> &v_in);
benson516 6:c362ed165c39 17 void reset(const vector<float> &v_in);
benson516 6:c362ed165c39 18
benson516 6:c362ed165c39 19 private:
benson516 6:c362ed165c39 20 size_t n;
benson516 6:c362ed165c39 21 float Ts;
benson516 6:c362ed165c39 22 float cutOff_freq_Hz; // Hz
benson516 6:c362ed165c39 23 float alpha_Ts;
benson516 6:c362ed165c39 24 float One_alpha_Ts;
benson516 6:c362ed165c39 25
benson516 6:c362ed165c39 26 // Flag
benson516 6:c362ed165c39 27 bool Flag_Init;
benson516 6:c362ed165c39 28
benson516 6:c362ed165c39 29 //
benson516 6:c362ed165c39 30 vector<float> zeros; // Zero vector [0;0;0]
benson516 6:c362ed165c39 31 };
benson516 10:166006e89252 32 */
benson516 0:8126c86bac2a 33
benson516 0:8126c86bac2a 34 // Class
benson516 0:8126c86bac2a 35 class ATTITUDE{
benson516 0:8126c86bac2a 36 public:
benson516 6:c362ed165c39 37
benson516 0:8126c86bac2a 38 // Variables
benson516 1:edc7ccfc5562 39 float alpha; // Convergent rate, rad/sec.
benson516 6:c362ed165c39 40 float one_over_gamma; // 1/gamma, one_over_gamma == 0 means no estimation on gyro bias
benson516 1:edc7ccfc5562 41 float Ts; // Sampling time, sec.
benson516 6:c362ed165c39 42 bool enable_biasEst; // Enable the gyro-bias estimation capability
benson516 8:3882cb4be9d3 43 bool enable_magEst; // Enable the estimation of magenetic field
benson516 6:c362ed165c39 44
benson516 6:c362ed165c39 45 // The map from "real" coordinate to "here" coordinate
benson516 6:c362ed165c39 46 // eg. accMap_real2here = [3,-1,-2];
benson516 6:c362ed165c39 47 // means: real -> here
benson516 6:c362ed165c39 48 // 1 x z 3
benson516 6:c362ed165c39 49 // 2 y -x -1
benson516 6:c362ed165c39 50 // 3 z -y -2
benson516 6:c362ed165c39 51 vector<int> accMap_real2here;
benson516 8:3882cb4be9d3 52 vector<int> magMap_real2here;
benson516 6:c362ed165c39 53 vector<int> gyroMap_real2here;
benson516 6:c362ed165c39 54
benson516 8:3882cb4be9d3 55 vector<float> xg_est; // Estimated g-vector
benson516 8:3882cb4be9d3 56 vector<float> xm_est; // Estimated m-vector (magnetic field)
benson516 8:3882cb4be9d3 57 // vector<float> x_est; // Estimated state
benson516 6:c362ed165c39 58 vector<float> gyroBias_est; // The estimated gyro bias in each channel
benson516 6:c362ed165c39 59 vector<float> omega; // Rotation speed in body-fixed frame
benson516 8:3882cb4be9d3 60 //
benson516 8:3882cb4be9d3 61 vector<float> y_acce; // Accelerometer outputs
benson516 8:3882cb4be9d3 62 vector<float> y_mag; // Magnetometer outputs
benson516 8:3882cb4be9d3 63
benson516 8:3882cb4be9d3 64 // No use, may be deleted
benson516 8:3882cb4be9d3 65 // vector<float> w_cross_ys; // omega X ys
benson516 6:c362ed165c39 66 vector<float> ys_cross_x_ys; // ys X (x_est - ys)
benson516 6:c362ed165c39 67
benson516 6:c362ed165c39 68 // Eular angles, in rad/s
benson516 6:c362ed165c39 69 float pitch;
benson516 6:c362ed165c39 70 float roll;
benson516 6:c362ed165c39 71 float yaw;
benson516 11:394a59f3b1f6 72 //
benson516 11:394a59f3b1f6 73 float pitchRate;
benson516 11:394a59f3b1f6 74 float rollRate;
benson516 11:394a59f3b1f6 75 float yawRate;
benson516 6:c362ed165c39 76
benson516 6:c362ed165c39 77
benson516 0:8126c86bac2a 78 // Constructor:
benson516 1:edc7ccfc5562 79 // Initialize the estimator
benson516 9:84fad91d3587 80 ATTITUDE(float alpha_in, float Ts_in); // alpha in rad/sec., Ts in sec.
benson516 6:c362ed165c39 81
benson516 0:8126c86bac2a 82 // Methods
benson516 7:6fc812e342e6 83 // Get the estimation results in Eular angle
benson516 7:6fc812e342e6 84 // vector <--> Eular angle
benson516 8:3882cb4be9d3 85 void Vectors_to_EulerAngle(const vector<float> &vg_in, const vector<float> &vm_in);
benson516 6:c362ed165c39 86 //float* EulerAngle_to_gVector(float Eu_in[]);
benson516 6:c362ed165c39 87
benson516 7:6fc812e342e6 88 // Setting parameters
benson516 7:6fc812e342e6 89 // Set L1, the diagonal matrix
benson516 7:6fc812e342e6 90 void Set_L1_diag(float alpha_in); // set diagnal element of gain matrix
benson516 9:84fad91d3587 91 void enable_gyroBiasEst(float gamma_in); // Enable the gyro-bias estimation
benson516 6:c362ed165c39 92
benson516 0:8126c86bac2a 93 // Estimator
benson516 8:3882cb4be9d3 94 void Init(void); // Let x_est = ys
benson516 8:3882cb4be9d3 95 // Two version of estimator, with/without magenetic field estimation
benson516 8:3882cb4be9d3 96 void iterateOnce(const vector<float> &y_acce_in, const vector<float> &omega_in); // Main alogorithm
benson516 8:3882cb4be9d3 97 void iterateOnce(const vector<float> &y_acce_in, const vector<float> &y_mag_in, const vector<float> &omega_in); // Main alogorithm
benson516 7:6fc812e342e6 98 // Get the results
benson516 6:c362ed165c39 99 void getEstimation_realCoordinate(vector<float> &V_out);
benson516 6:c362ed165c39 100 float pitch_deg(void);
benson516 6:c362ed165c39 101 float roll_deg(void);
benson516 6:c362ed165c39 102 float yaw_deg(void);
benson516 6:c362ed165c39 103
benson516 6:c362ed165c39 104 // Unit transformation
benson516 6:c362ed165c39 105 float pi; // pi = 3.1415926
benson516 6:c362ed165c39 106 float deg2rad; // = 3.1415926/180.0;
benson516 6:c362ed165c39 107 float rad2deg; // = 180.0/3.1415926;
benson516 7:6fc812e342e6 108 float gravity; // = 9.81 m/s^2
benson516 0:8126c86bac2a 109
benson516 0:8126c86bac2a 110 private:
benson516 6:c362ed165c39 111 // Dimension
benson516 6:c362ed165c39 112 size_t n;
benson516 6:c362ed165c39 113
benson516 5:01e322f4158f 114 // Variables
benson516 6:c362ed165c39 115 vector<float> unit_nx; // (-x) direction [-1;0;0]
benson516 6:c362ed165c39 116 vector<float> unit_ny; // (-y) direction [0;-1;0]
benson516 6:c362ed165c39 117 vector<float> unit_nz; // (-z) direction [0;0;-1]
benson516 6:c362ed165c39 118 vector<float> zeros; // Zero vector [0;0;0]
benson516 1:edc7ccfc5562 119 //
benson516 6:c362ed165c39 120 size_t init_flag; // Flag for displaying initialization status
benson516 1:edc7ccfc5562 121 // float _omega_x[3][3]; // Skew symmetric matrix of omega
benson516 6:c362ed165c39 122
benson516 6:c362ed165c39 123 vector<float> L1_diag; // Diagonal vector of gain matrix L1
benson516 6:c362ed165c39 124
benson516 7:6fc812e342e6 125 //
benson516 7:6fc812e342e6 126 // Input/output coordinate transformations within the different definitions between the "real" one and the "here" one
benson516 7:6fc812e342e6 127 void InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here);
benson516 7:6fc812e342e6 128 void OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here);
benson516 7:6fc812e342e6 129
benson516 7:6fc812e342e6 130
benson516 7:6fc812e342e6 131 // The kernel of the estimation process
benson516 7:6fc812e342e6 132 void EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_);
benson516 7:6fc812e342e6 133 void updateGyroBiasEst(void);
benson516 0:8126c86bac2a 134
benson516 7:6fc812e342e6 135 // Utilities
benson516 7:6fc812e342e6 136 // vector operation
benson516 7:6fc812e342e6 137 //float* Vector_to_SkewSymmetry(float v_in[]);
benson516 7:6fc812e342e6 138 //float* SkewSymmetry_to_Vector(float M_in[3][]);
benson516 7:6fc812e342e6 139 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 140 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 141 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
benson516 7:6fc812e342e6 142 float Get_Vector3Norm(const vector<float> &v_in);
benson516 7:6fc812e342e6 143 void Normolization(vector<float> &V_out, const vector<float> &V_in);
benson516 7:6fc812e342e6 144
benson516 7:6fc812e342e6 145 // Low-pass filter, vector version
benson516 8:3882cb4be9d3 146 LPF_vector lpfv_y_acce;
benson516 8:3882cb4be9d3 147 LPF_vector lpfv_y_mag;
benson516 6:c362ed165c39 148 LPF_vector lpfv_w;
benson516 0:8126c86bac2a 149 };
benson516 6:c362ed165c39 150
benson516 6:c362ed165c39 151 #endif // _ATTITUDE_ESTIMATION_H_