Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

ATTITUDE_ESTIMATION.cpp@12:058a9edb664e, 2017-01-05 (annotated)

Committer:
benson516
Date:
Thu Jan 05 11:14:20 2017 +0000
Revision:
12:058a9edb664e
Parent:
11:394a59f3b1f6
Child:
13:16bc19155f54
Add calculating the rate of each Eular angle

Who changed what in which revision?

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