00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "imu_complementary_filter/complementary_filter.h"
00033
00034 #include <cstdio>
00035 #include <cmath>
00036 #include <iostream>
00037
00038 namespace imu_tools {
00039
00040 const double ComplementaryFilter::kGravity = 9.81;
00041 const double ComplementaryFilter::gamma_ = 0.01;
00042
00043 const double ComplementaryFilter::kAngularVelocityThreshold = 0.2;
00044 const double ComplementaryFilter::kAccelerationThreshold = 0.1;
00045 const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01;
00046
00047 ComplementaryFilter::ComplementaryFilter() :
00048 gain_acc_(0.01),
00049 gain_mag_(0.01),
00050 bias_alpha_(0.01),
00051 do_bias_estimation_(true),
00052 do_adaptive_gain_(false),
00053 initialized_(false),
00054 steady_state_(false),
00055 q0_(1), q1_(0), q2_(0), q3_(0),
00056 wx_prev_(0), wy_prev_(0), wz_prev_(0),
00057 wx_bias_(0), wy_bias_(0), wz_bias_(0) { }
00058
00059 ComplementaryFilter::~ComplementaryFilter() { }
00060
00061 void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation)
00062 {
00063 do_bias_estimation_ = do_bias_estimation;
00064 }
00065
00066 bool ComplementaryFilter::getDoBiasEstimation() const
00067 {
00068 return do_bias_estimation_;
00069 }
00070
00071 void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain)
00072 {
00073 do_adaptive_gain_ = do_adaptive_gain;
00074 }
00075
00076 bool ComplementaryFilter::getDoAdaptiveGain() const
00077 {
00078 return do_adaptive_gain_;
00079 }
00080
00081 bool ComplementaryFilter::setGainAcc(double gain)
00082 {
00083 if (gain >= 0 && gain <= 1.0)
00084 {
00085 gain_acc_ = gain;
00086 return true;
00087 }
00088 else
00089 return false;
00090 }
00091 bool ComplementaryFilter::setGainMag(double gain)
00092 {
00093 if (gain >= 0 && gain <= 1.0)
00094 {
00095 gain_mag_ = gain;
00096 return true;
00097 }
00098 else
00099 return false;
00100 }
00101
00102 double ComplementaryFilter::getGainAcc() const
00103 {
00104 return gain_acc_;
00105 }
00106
00107 double ComplementaryFilter::getGainMag() const
00108 {
00109 return gain_mag_;
00110 }
00111
00112 bool ComplementaryFilter::getSteadyState() const
00113 {
00114 return steady_state_;
00115 }
00116
00117 bool ComplementaryFilter::setBiasAlpha(double bias_alpha)
00118 {
00119 if (bias_alpha >= 0 && bias_alpha <= 1.0)
00120 {
00121 bias_alpha_ = bias_alpha;
00122 return true;
00123 }
00124 else
00125 return false;
00126 }
00127
00128 double ComplementaryFilter::getBiasAlpha() const
00129 {
00130 return bias_alpha_;
00131 }
00132
00133 void ComplementaryFilter::setOrientation(
00134 double q0, double q1, double q2, double q3)
00135 {
00136
00137 invertQuaternion(q0, q1, q2, q3, q0_, q1_, q2_, q3_);
00138 }
00139
00140
00141 double ComplementaryFilter::getAngularVelocityBiasX() const
00142 {
00143 return wx_bias_;
00144 }
00145
00146 double ComplementaryFilter::getAngularVelocityBiasY() const
00147 {
00148 return wy_bias_;
00149 }
00150
00151 double ComplementaryFilter::getAngularVelocityBiasZ() const
00152 {
00153 return wz_bias_;
00154 }
00155
00156 void ComplementaryFilter::update(double ax, double ay, double az,
00157 double wx, double wy, double wz,
00158 double dt)
00159 {
00160 if (!initialized_)
00161 {
00162
00163 getMeasurement(ax, ay, az,
00164 q0_, q1_, q2_, q3_);
00165 initialized_ = true;
00166 return;
00167 }
00168
00169
00170 if (do_bias_estimation_)
00171 updateBiases(ax, ay, az, wx, wy, wz);
00172
00173
00174 double q0_pred, q1_pred, q2_pred, q3_pred;
00175 getPrediction(wx, wy, wz, dt,
00176 q0_pred, q1_pred, q2_pred, q3_pred);
00177
00178
00179
00180
00181 double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
00182 getAccCorrection(ax, ay, az,
00183 q0_pred, q1_pred, q2_pred, q3_pred,
00184 dq0_acc, dq1_acc, dq2_acc, dq3_acc);
00185
00186 double gain;
00187 if (do_adaptive_gain_)
00188 {
00189 gain = getAdaptiveGain(gain_acc_, ax, ay, az);
00190
00191 }
00192 else
00193 {
00194 gain = gain_acc_;
00195
00196 }
00197
00198 scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
00199
00200 quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
00201 dq0_acc, dq1_acc, dq2_acc, dq3_acc,
00202 q0_, q1_, q2_, q3_);
00203
00204 normalizeQuaternion(q0_, q1_, q2_, q3_);
00205 }
00206
00207 void ComplementaryFilter::update(double ax, double ay, double az,
00208 double wx, double wy, double wz,
00209 double mx, double my, double mz,
00210 double dt)
00211 {
00212 if (!initialized_)
00213 {
00214
00215 getMeasurement(ax, ay, az,
00216 mx, my, mz,
00217 q0_, q1_, q2_, q3_);
00218 initialized_ = true;
00219 return;
00220 }
00221
00222
00223 if (do_bias_estimation_)
00224 updateBiases(ax, ay, az, wx, wy, wz);
00225
00226
00227 double q0_pred, q1_pred, q2_pred, q3_pred;
00228 getPrediction(wx, wy, wz, dt,
00229 q0_pred, q1_pred, q2_pred, q3_pred);
00230
00231
00232
00233
00234 double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
00235 getAccCorrection(ax, ay, az,
00236 q0_pred, q1_pred, q2_pred, q3_pred,
00237 dq0_acc, dq1_acc, dq2_acc, dq3_acc);
00238 double alpha = gain_acc_;
00239 if (do_adaptive_gain_)
00240 alpha = getAdaptiveGain(gain_acc_, ax, ay, az);
00241 scaleQuaternion(alpha, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
00242
00243 double q0_temp, q1_temp, q2_temp, q3_temp;
00244 quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
00245 dq0_acc, dq1_acc, dq2_acc, dq3_acc,
00246 q0_temp, q1_temp, q2_temp, q3_temp);
00247
00248
00249
00250
00251 double dq0_mag, dq1_mag, dq2_mag, dq3_mag;
00252 getMagCorrection(mx, my, mz,
00253 q0_temp, q1_temp, q2_temp, q3_temp,
00254 dq0_mag, dq1_mag, dq2_mag, dq3_mag);
00255
00256 scaleQuaternion(gain_mag_, dq0_mag, dq1_mag, dq2_mag, dq3_mag);
00257
00258 quaternionMultiplication(q0_temp, q1_temp, q2_temp, q3_temp,
00259 dq0_mag, dq1_mag, dq2_mag, dq3_mag,
00260 q0_, q1_, q2_, q3_);
00261
00262 normalizeQuaternion(q0_, q1_, q2_, q3_);
00263 }
00264
00265 bool ComplementaryFilter::checkState(double ax, double ay, double az,
00266 double wx, double wy, double wz) const
00267 {
00268 double acc_magnitude = sqrt(ax*ax + ay*ay + az*az);
00269 if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold)
00270 return false;
00271
00272 if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold ||
00273 fabs(wy - wy_prev_) > kDeltaAngularVelocityThreshold ||
00274 fabs(wz - wz_prev_) > kDeltaAngularVelocityThreshold)
00275 return false;
00276
00277 if (fabs(wx - wx_bias_) > kAngularVelocityThreshold ||
00278 fabs(wy - wy_bias_) > kAngularVelocityThreshold ||
00279 fabs(wz - wz_bias_) > kAngularVelocityThreshold)
00280 return false;
00281
00282 return true;
00283 }
00284
00285 void ComplementaryFilter::updateBiases(double ax, double ay, double az,
00286 double wx, double wy, double wz)
00287 {
00288 steady_state_ = checkState(ax, ay, az, wx, wy, wz);
00289
00290 if (steady_state_)
00291 {
00292 wx_bias_ += bias_alpha_ * (wx - wx_bias_);
00293 wy_bias_ += bias_alpha_ * (wy - wy_bias_);
00294 wz_bias_ += bias_alpha_ * (wz - wz_bias_);
00295 }
00296
00297 wx_prev_ = wx;
00298 wy_prev_ = wy;
00299 wz_prev_ = wz;
00300 }
00301
00302 void ComplementaryFilter::getPrediction(
00303 double wx, double wy, double wz, double dt,
00304 double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const
00305 {
00306 double wx_unb = wx - wx_bias_;
00307 double wy_unb = wy - wy_bias_;
00308 double wz_unb = wz - wz_bias_;
00309
00310 q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_);
00311 q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_);
00312 q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_);
00313 q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_);
00314
00315 normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred);
00316 }
00317
00318 void ComplementaryFilter::getMeasurement(
00319 double ax, double ay, double az,
00320 double mx, double my, double mz,
00321 double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
00322 {
00323
00324
00325
00326 double q0_acc, q1_acc, q2_acc, q3_acc;
00327
00328
00329 normalizeVector(ax, ay, az);
00330 if (az >=0)
00331 {
00332 q0_acc = sqrt((az + 1) * 0.5);
00333 q1_acc = -ay/(2.0 * q0_acc);
00334 q2_acc = ax/(2.0 * q0_acc);
00335 q3_acc = 0;
00336 }
00337 else
00338 {
00339 double X = sqrt((1 - az) * 0.5);
00340 q0_acc = -ay/(2.0 * X);
00341 q1_acc = X;
00342 q2_acc = 0;
00343 q3_acc = ax/(2.0 * X);
00344 }
00345
00346
00347
00348
00349 double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx +
00350 2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz;
00351 double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc +
00352 q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz;
00353
00354
00355
00356 double gamma = lx*lx + ly*ly;
00357 double beta = sqrt(gamma + lx*sqrt(gamma));
00358 double q0_mag = beta / (sqrt(2.0 * gamma));
00359 double q3_mag = ly / (sqrt(2.0) * beta);
00360
00361
00362
00363
00364 quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc,
00365 q0_mag, 0, 0, q3_mag,
00366 q0_meas, q1_meas, q2_meas, q3_meas );
00367
00368
00369
00370
00371 }
00372
00373
00374 void ComplementaryFilter::getMeasurement(
00375 double ax, double ay, double az,
00376 double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
00377 {
00378
00379
00380
00381
00382
00383 normalizeVector(ax, ay, az);
00384
00385 if (az >=0)
00386 {
00387 q0_meas = sqrt((az + 1) * 0.5);
00388 q1_meas = -ay/(2.0 * q0_meas);
00389 q2_meas = ax/(2.0 * q0_meas);
00390 q3_meas = 0;
00391 }
00392 else
00393 {
00394 double X = sqrt((1 - az) * 0.5);
00395 q0_meas = -ay/(2.0 * X);
00396 q1_meas = X;
00397 q2_meas = 0;
00398 q3_meas = ax/(2.0 * X);
00399 }
00400 }
00401
00402 void ComplementaryFilter::getAccCorrection(
00403 double ax, double ay, double az,
00404 double p0, double p1, double p2, double p3,
00405 double& dq0, double& dq1, double& dq2, double& dq3)
00406 {
00407
00408 normalizeVector(ax, ay, az);
00409
00410
00411
00412 double gx, gy, gz;
00413 rotateVectorByQuaternion(ax, ay, az,
00414 p0, -p1, -p2, -p3,
00415 gx, gy, gz);
00416
00417
00418 dq0 = sqrt((gz + 1) * 0.5);
00419 dq1 = -gy/(2.0 * dq0);
00420 dq2 = gx/(2.0 * dq0);
00421 dq3 = 0.0;
00422 }
00423
00424 void ComplementaryFilter::getMagCorrection(
00425 double mx, double my, double mz,
00426 double p0, double p1, double p2, double p3,
00427 double& dq0, double& dq1, double& dq2, double& dq3)
00428 {
00429
00430
00431 double lx, ly, lz;
00432 rotateVectorByQuaternion(mx, my, mz,
00433 p0, -p1, -p2, -p3,
00434 lx, ly, lz);
00435
00436
00437
00438 double gamma = lx*lx + ly*ly;
00439 double beta = sqrt(gamma + lx*sqrt(gamma));
00440 dq0 = beta / (sqrt(2.0 * gamma));
00441 dq1 = 0.0;
00442 dq2 = 0.0;
00443 dq3 = ly / (sqrt(2.0) * beta);
00444 }
00445
00446 void ComplementaryFilter::getOrientation(
00447 double& q0, double& q1, double& q2, double& q3) const
00448 {
00449
00450 invertQuaternion(q0_, q1_, q2_, q3_, q0, q1, q2, q3);
00451 }
00452
00453 double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, double az)
00454 {
00455 double a_mag = sqrt(ax*ax + ay*ay + az*az);
00456 double error = fabs(a_mag - kGravity)/kGravity;
00457 double factor;
00458 double error1 = 0.1;
00459 double error2 = 0.2;
00460 double m = 1.0/(error1 - error2);
00461 double b = 1.0 - m*error1;
00462 if (error < error1)
00463 factor = 1.0;
00464 else if (error < error2)
00465 factor = m*error + b;
00466 else
00467 factor = 0.0;
00468
00469 return factor*alpha;
00470 }
00471
00472 void normalizeVector(double& x, double& y, double& z)
00473 {
00474 double norm = sqrt(x*x + y*y + z*z);
00475
00476 x /= norm;
00477 y /= norm;
00478 z /= norm;
00479 }
00480
00481 void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
00482 {
00483 double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
00484 q0 /= norm;
00485 q1 /= norm;
00486 q2 /= norm;
00487 q3 /= norm;
00488 }
00489
00490 void invertQuaternion(
00491 double q0, double q1, double q2, double q3,
00492 double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv)
00493 {
00494
00495 q0_inv = q0;
00496 q1_inv = -q1;
00497 q2_inv = -q2;
00498 q3_inv = -q3;
00499 }
00500
00501 void scaleQuaternion(
00502 double gain,
00503 double& dq0, double& dq1, double& dq2, double& dq3)
00504 {
00505 if (dq0 < 0.0)
00506 {
00507
00508 double angle = acos(dq0);
00509 double A = sin(angle*(1.0 - gain))/sin(angle);
00510 double B = sin(angle * gain)/sin(angle);
00511 dq0 = A + B * dq0;
00512 dq1 = B * dq1;
00513 dq2 = B * dq2;
00514 dq3 = B * dq3;
00515 }
00516 else
00517 {
00518
00519 dq0 = (1.0 - gain) + gain * dq0;
00520 dq1 = gain * dq1;
00521 dq2 = gain * dq2;
00522 dq3 = gain * dq3;
00523 }
00524
00525 normalizeQuaternion(dq0, dq1, dq2, dq3);
00526 }
00527
00528 void quaternionMultiplication(
00529 double p0, double p1, double p2, double p3,
00530 double q0, double q1, double q2, double q3,
00531 double& r0, double& r1, double& r2, double& r3)
00532 {
00533
00534 r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3;
00535 r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2;
00536 r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1;
00537 r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0;
00538 }
00539
00540 void rotateVectorByQuaternion(
00541 double x, double y, double z,
00542 double q0, double q1, double q2, double q3,
00543 double& vx, double& vy, double& vz)
00544 {
00545 vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z;
00546 vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z;
00547 vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z;
00548 }
00549
00550
00551 }