Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

ATTITUDE_ESTIMATION.cpp@7:6fc812e342e6, 2016-12-27 (annotated)

Committer:
benson516
Date:
Tue Dec 27 07:43:25 2016 +0000
Revision:
7:6fc812e342e6
Parent:
6:c362ed165c39
Child:
8:3882cb4be9d3
Build up the kernel function

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:8126c86bac2a 1 #include "ATTITUDE_ESTIMATION.h"
benson516 0:8126c86bac2a 2
benson516 6:c362ed165c39 3 //=====================LPF ====================//
benson516 6:c362ed165c39 4 LPF_vector::LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in)
benson516 6:c362ed165c39 5 {
benson516 6:c362ed165c39 6 n = dimension;
benson516 6:c362ed165c39 7 Ts = samplingTime;
benson516 6:c362ed165c39 8 cutOff_freq_Hz = cutOff_freq_Hz_in;
benson516 6:c362ed165c39 9 alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
benson516 6:c362ed165c39 10 One_alpha_Ts = 1.0 - alpha_Ts;
benson516 6:c362ed165c39 11
benson516 6:c362ed165c39 12 zeros.assign(n, 0.0);
benson516 6:c362ed165c39 13
benson516 6:c362ed165c39 14 output = zeros;
benson516 6:c362ed165c39 15
benson516 6:c362ed165c39 16 //
benson516 6:c362ed165c39 17 Flag_Init = false;
benson516 6:c362ed165c39 18 }
benson516 6:c362ed165c39 19
benson516 6:c362ed165c39 20 vector<float> LPF_vector::filter(const vector<float> &input)
benson516 6:c362ed165c39 21 {
benson516 6:c362ed165c39 22 // Initialization
benson516 6:c362ed165c39 23 if (!Flag_Init){
benson516 6:c362ed165c39 24 reset(input);
benson516 6:c362ed165c39 25 Flag_Init = true;
benson516 6:c362ed165c39 26 return output;
benson516 6:c362ed165c39 27 }
benson516 6:c362ed165c39 28
benson516 6:c362ed165c39 29 for (size_t i = 0; i < n; ++i){
benson516 6:c362ed165c39 30 // output = One_alpha_Ts*output + alpha_Ts*input;
benson516 6:c362ed165c39 31 output[i] += alpha_Ts*(input[i] - output[i]);
benson516 6:c362ed165c39 32 }
benson516 6:c362ed165c39 33
benson516 6:c362ed165c39 34 return output;
benson516 6:c362ed165c39 35 }
benson516 6:c362ed165c39 36 void LPF_vector::reset(const vector<float> &input)
benson516 6:c362ed165c39 37 {
benson516 6:c362ed165c39 38 // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
benson516 6:c362ed165c39 39 output = input;
benson516 6:c362ed165c39 40 return;
benson516 6:c362ed165c39 41 }
benson516 6:c362ed165c39 42
benson516 6:c362ed165c39 43 //-------------------------------------------------------//
benson516 6:c362ed165c39 44 ATTITUDE::ATTITUDE(float alpha_in, float one_over_gamma_in, float Ts_in):
benson516 6:c362ed165c39 45 alpha(alpha_in),
benson516 6:c362ed165c39 46 one_over_gamma(one_over_gamma_in),
benson516 6:c362ed165c39 47 Ts(Ts_in),
benson516 6:c362ed165c39 48 lpfv_ys(3, Ts_in, 10.0), // Input filter
benson516 6:c362ed165c39 49 lpfv_w(3, Ts_in, 200.0) // Input filter
benson516 0:8126c86bac2a 50 {
benson516 6:c362ed165c39 51 // Dimension
benson516 6:c362ed165c39 52 n = 3;
benson516 6:c362ed165c39 53 //
benson516 6:c362ed165c39 54 init_flag = 0; // Uninitialized
benson516 6:c362ed165c39 55
benson516 6:c362ed165c39 56 // Default: close the gyro-bias estimation
benson516 6:c362ed165c39 57 enable_biasEst = false;
benson516 6:c362ed165c39 58
benson516 6:c362ed165c39 59 // Unit transformation
benson516 6:c362ed165c39 60 pi = 3.1415926;
benson516 6:c362ed165c39 61 deg2rad = pi/180.0;
benson516 6:c362ed165c39 62 rad2deg = 180.0/pi;
benson516 7:6fc812e342e6 63 gravity = 9.81; // m/s^2
benson516 6:c362ed165c39 64
benson516 6:c362ed165c39 65 // The map from "real" coordinate to "here" coordinate
benson516 6:c362ed165c39 66 // eg. accMap_real2here = [3,-1,-2];
benson516 6:c362ed165c39 67 // means: real -> here
benson516 6:c362ed165c39 68 // 1 x z 3
benson516 6:c362ed165c39 69 // 2 y -x -1
benson516 6:c362ed165c39 70 // 3 z -y -2
benson516 6:c362ed165c39 71 // int accmap_temp[] = {3,-1,-2};
benson516 6:c362ed165c39 72 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
benson516 6:c362ed165c39 73 // int accmap_temp[] = {1, 2, 3};
benson516 6:c362ed165c39 74 //
benson516 6:c362ed165c39 75 int gyroMap_temp[] = {3,-1,-2};
benson516 6:c362ed165c39 76 accMap_real2here.assign(accmap_temp, accmap_temp+n);
benson516 6:c362ed165c39 77 gyroMap_real2here.assign(gyroMap_temp, gyroMap_temp+n);
benson516 6:c362ed165c39 78
benson516 6:c362ed165c39 79 // zeros
benson516 6:c362ed165c39 80 zeros.assign(n,0.0);
benson516 6:c362ed165c39 81 // unit_nz
benson516 6:c362ed165c39 82 unit_nx = zeros;
benson516 6:c362ed165c39 83 unit_ny = zeros;
benson516 6:c362ed165c39 84 unit_nz = zeros;
benson516 6:c362ed165c39 85 unit_nx[0] = -1; // negative x
benson516 6:c362ed165c39 86 unit_ny[1] = -1; // negative y
benson516 6:c362ed165c39 87 unit_nz[2] = -1; // negative z
benson516 6:c362ed165c39 88
benson516 6:c362ed165c39 89 // States
benson516 6:c362ed165c39 90 x_est = unit_nx; // x is pointing downward
benson516 6:c362ed165c39 91 gyroBias_est = zeros;
benson516 6:c362ed165c39 92 omega = zeros;
benson516 6:c362ed165c39 93 ys = zeros;
benson516 6:c362ed165c39 94 w_cross_ys = zeros; // omega X ys
benson516 6:c362ed165c39 95 ys_cross_x_ys = zeros; // ys X (x_est - ys)
benson516 6:c362ed165c39 96
benson516 6:c362ed165c39 97 // Eular angles, in rad/s
benson516 6:c362ed165c39 98 pitch = 0.0;
benson516 6:c362ed165c39 99 roll = 0.0;
benson516 6:c362ed165c39 100 yaw = 0.0;
benson516 6:c362ed165c39 101
benson516 6:c362ed165c39 102
benson516 6:c362ed165c39 103 // Gain matrix
benson516 1:edc7ccfc5562 104 Set_L1_diag(alpha);
benson516 6:c362ed165c39 105
benson516 6:c362ed165c39 106 }
benson516 7:6fc812e342e6 107 // Public methods
benson516 7:6fc812e342e6 108 void ATTITUDE::gVector_to_EulerAngle(const vector<float> &v_in)
benson516 7:6fc812e342e6 109 {
benson516 7:6fc812e342e6 110 //
benson516 7:6fc812e342e6 111 // This function should be customized according to the definition of coordinate system
benson516 7:6fc812e342e6 112 //
benson516 6:c362ed165c39 113
benson516 7:6fc812e342e6 114 /*
benson516 7:6fc812e342e6 115 // Here we follow the definition in bicycle paper
benson516 7:6fc812e342e6 116 yaw = 0.0; // phi, yaw
benson516 7:6fc812e342e6 117 roll = atan2(-v_in[1],v_in[0]); // theta, roll
benson516 7:6fc812e342e6 118 pitch = atan2(cos(roll)*v_in[2],v_in[0]); // psi, pitch
benson516 7:6fc812e342e6 119 */
benson516 7:6fc812e342e6 120
benson516 7:6fc812e342e6 121 // Eular angle: 1-3-2, zs is pointing forwasd
benson516 7:6fc812e342e6 122 yaw = 0.0; // phi, yaw
benson516 7:6fc812e342e6 123 pitch = atan2(-v_in[2],-v_in[0]); // psi, pitch
benson516 7:6fc812e342e6 124 if (abs(pitch) < 0.7854){ // pi/4
benson516 7:6fc812e342e6 125 roll = atan2(cos(pitch)*v_in[1],-v_in[0]); // theta, roll
benson516 7:6fc812e342e6 126 }else{
benson516 7:6fc812e342e6 127 if (pitch >= 0.0)
benson516 7:6fc812e342e6 128 roll = atan2(sin(pitch)*v_in[1],-v_in[2]); // theta, roll
benson516 7:6fc812e342e6 129 else
benson516 7:6fc812e342e6 130 roll = atan2(-sin(pitch)*v_in[1],v_in[2]); // theta, roll
benson516 7:6fc812e342e6 131 }
benson516 7:6fc812e342e6 132
benson516 7:6fc812e342e6 133
benson516 7:6fc812e342e6 134
benson516 7:6fc812e342e6 135 }
benson516 7:6fc812e342e6 136 // Setting parameters
benson516 7:6fc812e342e6 137 // Set L1, the diagonal matrix
benson516 7:6fc812e342e6 138 void ATTITUDE::Set_L1_diag(float alpha_in) // set diagnal element of gain matrix
benson516 7:6fc812e342e6 139 {
benson516 7:6fc812e342e6 140 alpha = alpha_in;
benson516 7:6fc812e342e6 141 L1_diag.assign(n,alpha_in);
benson516 7:6fc812e342e6 142 }
benson516 7:6fc812e342e6 143 //
benson516 7:6fc812e342e6 144 void ATTITUDE::Init(const vector<float> &y_in) // Let _x_est = y_in
benson516 7:6fc812e342e6 145 {
benson516 7:6fc812e342e6 146 // ys = y_in;
benson516 7:6fc812e342e6 147 // Normolization(x_est,y_in); // x_est be set as normalized y_in
benson516 7:6fc812e342e6 148 x_est = y_in;
benson516 7:6fc812e342e6 149 ++init_flag;
benson516 7:6fc812e342e6 150 }
benson516 7:6fc812e342e6 151 void ATTITUDE::iterateOnce(const vector<float> &y_in, const vector<float> &omega_in) // Main alogorithm
benson516 7:6fc812e342e6 152 {
benson516 7:6fc812e342e6 153 InputMapping(ys, y_in, accMap_real2here);
benson516 7:6fc812e342e6 154 InputMapping(omega, omega_in, gyroMap_real2here);
benson516 7:6fc812e342e6 155
benson516 7:6fc812e342e6 156 // Input filter
benson516 7:6fc812e342e6 157 ys = lpfv_ys.filter(ys);
benson516 7:6fc812e342e6 158 // omega = lpfv_w.filter(omega);
benson516 7:6fc812e342e6 159
benson516 7:6fc812e342e6 160 // gyro-bias estimation
benson516 7:6fc812e342e6 161 if (enable_biasEst){
benson516 7:6fc812e342e6 162 omega = Get_VectorPlus(omega,gyroBias_est, true); // omega - gyroBias_est
benson516 7:6fc812e342e6 163 }
benson516 7:6fc812e342e6 164
benson516 7:6fc812e342e6 165 //
benson516 7:6fc812e342e6 166 if(init_flag < 3){
benson516 7:6fc812e342e6 167 Init(ys);
benson516 7:6fc812e342e6 168 }
benson516 7:6fc812e342e6 169 else{
benson516 7:6fc812e342e6 170 //
benson516 7:6fc812e342e6 171 /*
benson516 7:6fc812e342e6 172 Get_CrossProduct3(w_cross_ys,omega,ys);
benson516 7:6fc812e342e6 173 for(int i=0; i<3; i++){
benson516 7:6fc812e342e6 174 // x_est_plus[i] = x_est[i] + Ts*( L1_diag[i]*(ys[i] - x_est[i]) - w_cross_ys[i]);
benson516 7:6fc812e342e6 175 x_est[i] += Ts*( L1_diag[i]*(ys[i] - x_est[i]) - w_cross_ys[i]);
benson516 7:6fc812e342e6 176 }
benson516 7:6fc812e342e6 177 */
benson516 7:6fc812e342e6 178 // Estimation process
benson516 7:6fc812e342e6 179 EstimationKernel(x_est, ys, omega);
benson516 7:6fc812e342e6 180
benson516 7:6fc812e342e6 181 // gyro-bias estimation
benson516 7:6fc812e342e6 182 if (enable_biasEst){
benson516 7:6fc812e342e6 183 updateGyroBiasEst();
benson516 7:6fc812e342e6 184 }
benson516 7:6fc812e342e6 185 }
benson516 7:6fc812e342e6 186 //
benson516 7:6fc812e342e6 187 gVector_to_EulerAngle(x_est);
benson516 7:6fc812e342e6 188 }
benson516 7:6fc812e342e6 189 // transform the x_est into "real" coordinate
benson516 7:6fc812e342e6 190 void ATTITUDE::getEstimation_realCoordinate(vector<float> &V_out){
benson516 7:6fc812e342e6 191 OutputMapping(V_out,x_est,accMap_real2here);
benson516 7:6fc812e342e6 192 }
benson516 7:6fc812e342e6 193 // Get Eular angles
benson516 7:6fc812e342e6 194 float ATTITUDE::pitch_deg(void){
benson516 7:6fc812e342e6 195 return (rad2deg*pitch);
benson516 7:6fc812e342e6 196 }
benson516 7:6fc812e342e6 197 float ATTITUDE::roll_deg(void){
benson516 7:6fc812e342e6 198 return (rad2deg*roll);
benson516 7:6fc812e342e6 199 }
benson516 7:6fc812e342e6 200 float ATTITUDE::yaw_deg(void){
benson516 7:6fc812e342e6 201 return (rad2deg*yaw);
benson516 7:6fc812e342e6 202 }
benson516 7:6fc812e342e6 203
benson516 7:6fc812e342e6 204 // Private methods
benson516 7:6fc812e342e6 205 ////////////////////////////////////
benson516 7:6fc812e342e6 206 // Input/output coordinate transformations within the different definitions between the "real" one and the "here" one
benson516 6:c362ed165c39 207 // real -> here
benson516 6:c362ed165c39 208 void ATTITUDE::InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here){
benson516 6:c362ed165c39 209 // The map from "real" coordinate to "here" coordinate
benson516 6:c362ed165c39 210 // eg. accMap_real2here = [3,-1,-2];
benson516 6:c362ed165c39 211 // means: real -> here
benson516 6:c362ed165c39 212 // 1 x z 3
benson516 6:c362ed165c39 213 // 2 y -x -1
benson516 6:c362ed165c39 214 // 3 z -y -2
benson516 6:c362ed165c39 215 // vector<int> accMap_real2here = {3,-1,-2};
benson516 6:c362ed165c39 216 // vector<int> gyroMap_real2here = {3,-1,-2};
benson516 6:c362ed165c39 217
benson516 6:c362ed165c39 218 // Iterate through "real" coordinates
benson516 6:c362ed165c39 219 int idx_here = 1;
benson516 6:c362ed165c39 220 for (size_t i = 0; i < n; ++i){
benson516 6:c362ed165c39 221 idx_here = map_real2here[i];
benson516 6:c362ed165c39 222 if (idx_here > 0){
benson516 6:c362ed165c39 223 v_hereDef[idx_here-1] = v_realDef[i];
benson516 6:c362ed165c39 224 }else{
benson516 6:c362ed165c39 225 v_hereDef[-idx_here-1] = -1*v_realDef[i];
benson516 6:c362ed165c39 226 }
benson516 6:c362ed165c39 227 }
benson516 0:8126c86bac2a 228 }
benson516 6:c362ed165c39 229 // here -> real
benson516 6:c362ed165c39 230 void ATTITUDE::OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here){
benson516 6:c362ed165c39 231 // This is the inverse mapping of the InputMapping
benson516 6:c362ed165c39 232
benson516 6:c362ed165c39 233 // The map from "real" coordinate to "here" coordinate
benson516 6:c362ed165c39 234 // eg. accMap_real2here = [3,-1,-2];
benson516 6:c362ed165c39 235 // means: real -> here
benson516 6:c362ed165c39 236 // 1 x z 3
benson516 6:c362ed165c39 237 // 2 y -x -1
benson516 6:c362ed165c39 238 // 3 z -y -2
benson516 6:c362ed165c39 239 // vector<int> accMap_real2here = {3,-1,-2};
benson516 6:c362ed165c39 240 // vector<int> gyroMap_real2here = {3,-1,-2};
benson516 6:c362ed165c39 241
benson516 6:c362ed165c39 242 // Iterate through "real" coordinates
benson516 6:c362ed165c39 243 int idx_here = 1;
benson516 6:c362ed165c39 244 for (size_t i = 0; i < n; ++i){
benson516 6:c362ed165c39 245 idx_here = map_real2here[i];
benson516 6:c362ed165c39 246 if (idx_here > 0){
benson516 6:c362ed165c39 247 v_realDef[i] = v_hereDef[idx_here-1];
benson516 6:c362ed165c39 248 }else{
benson516 6:c362ed165c39 249 v_realDef[i] = -1*v_hereDef[-idx_here-1];
benson516 6:c362ed165c39 250 }
benson516 6:c362ed165c39 251 }
benson516 6:c362ed165c39 252 }
benson516 7:6fc812e342e6 253 // The kernel of the estimation process
benson516 7:6fc812e342e6 254 ////////////////////////////////////////
benson516 7:6fc812e342e6 255 void ATTITUDE::EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_){
benson516 7:6fc812e342e6 256 static vector<float> _w_cross_ys_;
benson516 7:6fc812e342e6 257 Get_CrossProduct3(_w_cross_ys_, _omega_, _ys_);
benson516 7:6fc812e342e6 258 for(int i=0; i<3; i++){
benson516 7:6fc812e342e6 259 // x_est_plus[i] = x_est[i] + Ts*( L1_diag[i]*(ys[i] - x_est[i]) - w_cross_ys[i]);
benson516 7:6fc812e342e6 260 _x_est_[i] += Ts*( L1_diag[i]*(_ys_[i] - _x_est_[i]) - _w_cross_ys_[i]);
benson516 6:c362ed165c39 261 }
benson516 6:c362ed165c39 262
benson516 7:6fc812e342e6 263 }
benson516 7:6fc812e342e6 264 void ATTITUDE::updateGyroBiasEst(void){ // Update the gyro bias estimation
benson516 7:6fc812e342e6 265 if (one_over_gamma == 0.0){
benson516 7:6fc812e342e6 266 return;
benson516 7:6fc812e342e6 267 }
benson516 7:6fc812e342e6 268 //
benson516 7:6fc812e342e6 269 Get_CrossProduct3(ys_cross_x_ys, ys, Get_VectorPlus(x_est,ys,true));
benson516 7:6fc812e342e6 270 //
benson516 7:6fc812e342e6 271 gyroBias_est = Get_VectorPlus(gyroBias_est, Get_VectorScalarMultiply(ys_cross_x_ys, (one_over_gamma)), true);
benson516 7:6fc812e342e6 272 }
benson516 6:c362ed165c39 273
benson516 7:6fc812e342e6 274 //////////////////////////////////////// end The kernal of the estimation process
benson516 7:6fc812e342e6 275
benson516 7:6fc812e342e6 276 // Utilities
benson516 7:6fc812e342e6 277 // vector operation
benson516 6:c362ed165c39 278 void ATTITUDE::Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b) // v_a X v_b
benson516 1:edc7ccfc5562 279 {
benson516 7:6fc812e342e6 280 // Check the size
benson516 7:6fc812e342e6 281 if (v_c.size() != n){
benson516 7:6fc812e342e6 282 v_c.resize(n);
benson516 7:6fc812e342e6 283 }
benson516 1:edc7ccfc5562 284 v_c[0] = (-v_a[2]*v_b[1]) + v_a[1]*v_b[2];
benson516 1:edc7ccfc5562 285 v_c[1] = v_a[2]*v_b[0] - v_a[0]*v_b[2];
benson516 6:c362ed165c39 286 v_c[2] = (-v_a[1]*v_b[0]) + v_a[0]*v_b[1];
benson516 1:edc7ccfc5562 287 }
benson516 6:c362ed165c39 288 vector<float> ATTITUDE::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
benson516 6:c362ed165c39 289 {
benson516 6:c362ed165c39 290 static vector<float> v_c(n);
benson516 6:c362ed165c39 291 for (size_t i = 0; i < n; ++i){
benson516 6:c362ed165c39 292 if (is_minus){
benson516 6:c362ed165c39 293 v_c[i] = v_a[i] - v_b[i];
benson516 6:c362ed165c39 294 }else{
benson516 6:c362ed165c39 295 v_c[i] = v_a[i] + v_b[i];
benson516 6:c362ed165c39 296 }
benson516 6:c362ed165c39 297 }
benson516 6:c362ed165c39 298 return v_c;
benson516 6:c362ed165c39 299 }
benson516 6:c362ed165c39 300 vector<float> ATTITUDE::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 6:c362ed165c39 301 {
benson516 6:c362ed165c39 302 static vector<float> v_c(n);
benson516 6:c362ed165c39 303 for (size_t i = 0; i < n; ++i){
benson516 6:c362ed165c39 304 v_c[i] = scale*v_a[i];
benson516 6:c362ed165c39 305
benson516 6:c362ed165c39 306 }
benson516 6:c362ed165c39 307 return v_c;
benson516 6:c362ed165c39 308 }
benson516 6:c362ed165c39 309 float ATTITUDE::Get_Vector3Norm(const vector<float> &v_in)
benson516 1:edc7ccfc5562 310 {
benson516 1:edc7ccfc5562 311 float temp = 0.0;
benson516 6:c362ed165c39 312
benson516 6:c362ed165c39 313 for (size_t i = 0; i < n; ++i)
benson516 1:edc7ccfc5562 314 temp += v_in[i]*v_in[i];
benson516 6:c362ed165c39 315 return sqrt(temp); // <- Should check if this function is available (?)
benson516 1:edc7ccfc5562 316 }
benson516 6:c362ed165c39 317 void ATTITUDE::Normolization(vector<float> &V_out, const vector<float> &V_in){
benson516 6:c362ed165c39 318 float norm = Get_Vector3Norm(V_in);
benson516 6:c362ed165c39 319 for (size_t i = 0; i < n; ++i){
benson516 6:c362ed165c39 320 V_out[i] = V_in[i]/norm;
benson516 6:c362ed165c39 321 }
benson516 6:c362ed165c39 322 }