complementary_filter.cpp
Go to the documentation of this file.
1 /*
2  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
3 
4  @section LICENSE
5  Copyright (c) 2015, City University of New York
6  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
7  All rights reserved.
8 
9  Redistribution and use in source and binary forms, with or without
10  modification, are permitted provided that the following conditions are met:
11  1. Redistributions of source code must retain the above copyright
12  notice, this list of conditions and the following disclaimer.
13  2. Redistributions in binary form must reproduce the above copyright
14  notice, this list of conditions and the following disclaimer in the
15  documentation and/or other materials provided with the distribution.
16  3. Neither the name of the City College of New York nor the
17  names of its contributors may be used to endorse or promote products
18  derived from this software without specific prior written permission.
19 
20  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
21  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
22  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
24  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
27  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
28  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31 
33 
34 #include <cstdio>
35 #include <cmath>
36 #include <iostream>
37 
38 namespace imu_tools {
39 
40 const double ComplementaryFilter::kGravity = 9.81;
41 const double ComplementaryFilter::gamma_ = 0.01;
42 // Bias estimation steady state thresholds
46 
48  gain_acc_(0.01),
49  gain_mag_(0.01),
50  bias_alpha_(0.01),
51  do_bias_estimation_(true),
52  do_adaptive_gain_(false),
53  initialized_(false),
54  steady_state_(false),
55  q0_(1), q1_(0), q2_(0), q3_(0),
56  wx_prev_(0), wy_prev_(0), wz_prev_(0),
57  wx_bias_(0), wy_bias_(0), wz_bias_(0) { }
58 
60 
61 void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation)
62 {
63  do_bias_estimation_ = do_bias_estimation;
64 }
65 
67 {
68  return do_bias_estimation_;
69 }
70 
71 void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain)
72 {
73  do_adaptive_gain_ = do_adaptive_gain;
74 }
75 
77 {
78  return do_adaptive_gain_;
79 }
80 
82 {
83  if (gain >= 0 && gain <= 1.0)
84  {
85  gain_acc_ = gain;
86  return true;
87  }
88  else
89  return false;
90 }
92 {
93  if (gain >= 0 && gain <= 1.0)
94  {
95  gain_mag_ = gain;
96  return true;
97  }
98  else
99  return false;
100 }
101 
103 {
104  return gain_acc_;
105 }
106 
108 {
109  return gain_mag_;
110 }
111 
113 {
114  return steady_state_;
115 }
116 
117 bool ComplementaryFilter::setBiasAlpha(double bias_alpha)
118 {
119  if (bias_alpha >= 0 && bias_alpha <= 1.0)
120  {
121  bias_alpha_ = bias_alpha;
122  return true;
123  }
124  else
125  return false;
126 }
127 
129 {
130  return bias_alpha_;
131 }
132 
134  double q0, double q1, double q2, double q3)
135 {
136  // Set the state to inverse (state is fixed wrt body).
137  invertQuaternion(q0, q1, q2, q3, q0_, q1_, q2_, q3_);
138 }
139 
140 
142 {
143  return wx_bias_;
144 }
145 
147 {
148  return wy_bias_;
149 }
150 
152 {
153  return wz_bias_;
154 }
155 
156 void ComplementaryFilter::update(double ax, double ay, double az,
157  double wx, double wy, double wz,
158  double dt)
159 {
160  if (!initialized_)
161  {
162  // First time - ignore prediction:
163  getMeasurement(ax, ay, az,
164  q0_, q1_, q2_, q3_);
165  initialized_ = true;
166  return;
167  }
168 
169  // Bias estimation.
171  updateBiases(ax, ay, az, wx, wy, wz);
172 
173  // Prediction.
174  double q0_pred, q1_pred, q2_pred, q3_pred;
175  getPrediction(wx, wy, wz, dt,
176  q0_pred, q1_pred, q2_pred, q3_pred);
177 
178  // Correction (from acc):
179  // q_ = q_pred * [(1-gain) * qI + gain * dq_acc]
180  // where qI = identity quaternion
181  double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
182  getAccCorrection(ax, ay, az,
183  q0_pred, q1_pred, q2_pred, q3_pred,
184  dq0_acc, dq1_acc, dq2_acc, dq3_acc);
185 
186  double gain;
187  if (do_adaptive_gain_)
188  {
189  gain = getAdaptiveGain(gain_acc_, ax, ay, az);
190 
191  }
192  else
193  {
194  gain = gain_acc_;
195 
196  }
197 
198  scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
199 
200  quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
201  dq0_acc, dq1_acc, dq2_acc, dq3_acc,
202  q0_, q1_, q2_, q3_);
203 
205 }
206 
207 void ComplementaryFilter::update(double ax, double ay, double az,
208  double wx, double wy, double wz,
209  double mx, double my, double mz,
210  double dt)
211 {
212  if (!initialized_)
213  {
214  // First time - ignore prediction:
215  getMeasurement(ax, ay, az,
216  mx, my, mz,
217  q0_, q1_, q2_, q3_);
218  initialized_ = true;
219  return;
220  }
221 
222  // Bias estimation.
224  updateBiases(ax, ay, az, wx, wy, wz);
225 
226  // Prediction.
227  double q0_pred, q1_pred, q2_pred, q3_pred;
228  getPrediction(wx, wy, wz, dt,
229  q0_pred, q1_pred, q2_pred, q3_pred);
230 
231  // Correction (from acc):
232  // q_temp = q_pred * [(1-gain) * qI + gain * dq_acc]
233  // where qI = identity quaternion
234  double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
235  getAccCorrection(ax, ay, az,
236  q0_pred, q1_pred, q2_pred, q3_pred,
237  dq0_acc, dq1_acc, dq2_acc, dq3_acc);
238  double alpha = gain_acc_;
239  if (do_adaptive_gain_)
240  alpha = getAdaptiveGain(gain_acc_, ax, ay, az);
241  scaleQuaternion(alpha, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
242 
243  double q0_temp, q1_temp, q2_temp, q3_temp;
244  quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
245  dq0_acc, dq1_acc, dq2_acc, dq3_acc,
246  q0_temp, q1_temp, q2_temp, q3_temp);
247 
248  // Correction (from mag):
249  // q_ = q_temp * [(1-gain) * qI + gain * dq_mag]
250  // where qI = identity quaternion
251  double dq0_mag, dq1_mag, dq2_mag, dq3_mag;
252  getMagCorrection(mx, my, mz,
253  q0_temp, q1_temp, q2_temp, q3_temp,
254  dq0_mag, dq1_mag, dq2_mag, dq3_mag);
255 
256  scaleQuaternion(gain_mag_, dq0_mag, dq1_mag, dq2_mag, dq3_mag);
257 
258  quaternionMultiplication(q0_temp, q1_temp, q2_temp, q3_temp,
259  dq0_mag, dq1_mag, dq2_mag, dq3_mag,
260  q0_, q1_, q2_, q3_);
261 
263 }
264 
265 bool ComplementaryFilter::checkState(double ax, double ay, double az,
266  double wx, double wy, double wz) const
267 {
268  double acc_magnitude = sqrt(ax*ax + ay*ay + az*az);
269  if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold)
270  return false;
271 
272  if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold ||
275  return false;
276 
277  if (fabs(wx - wx_bias_) > kAngularVelocityThreshold ||
278  fabs(wy - wy_bias_) > kAngularVelocityThreshold ||
279  fabs(wz - wz_bias_) > kAngularVelocityThreshold)
280  return false;
281 
282  return true;
283 }
284 
285 void ComplementaryFilter::updateBiases(double ax, double ay, double az,
286  double wx, double wy, double wz)
287 {
288  steady_state_ = checkState(ax, ay, az, wx, wy, wz);
289 
290  if (steady_state_)
291  {
292  wx_bias_ += bias_alpha_ * (wx - wx_bias_);
293  wy_bias_ += bias_alpha_ * (wy - wy_bias_);
294  wz_bias_ += bias_alpha_ * (wz - wz_bias_);
295  }
296 
297  wx_prev_ = wx;
298  wy_prev_ = wy;
299  wz_prev_ = wz;
300 }
301 
303  double wx, double wy, double wz, double dt,
304  double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const
305 {
306  double wx_unb = wx - wx_bias_;
307  double wy_unb = wy - wy_bias_;
308  double wz_unb = wz - wz_bias_;
309 
310  q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_);
311  q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_);
312  q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_);
313  q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_);
314 
315  normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred);
316 }
317 
319  double ax, double ay, double az,
320  double mx, double my, double mz,
321  double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
322 {
323  // q_acc is the quaternion obtained from the acceleration vector representing
324  // the orientation of the Global frame wrt the Local frame with arbitrary yaw
325  // (intermediary frame). q3_acc is defined as 0.
326  double q0_acc, q1_acc, q2_acc, q3_acc;
327 
328  // Normalize acceleration vector.
329  normalizeVector(ax, ay, az);
330  if (az >=0)
331  {
332  q0_acc = sqrt((az + 1) * 0.5);
333  q1_acc = -ay/(2.0 * q0_acc);
334  q2_acc = ax/(2.0 * q0_acc);
335  q3_acc = 0;
336  }
337  else
338  {
339  double X = sqrt((1 - az) * 0.5);
340  q0_acc = -ay/(2.0 * X);
341  q1_acc = X;
342  q2_acc = 0;
343  q3_acc = ax/(2.0 * X);
344  }
345 
346  // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary
347  // frame by the inverse of q_acc.
348  // l = R(q_acc)^-1 m
349  double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx +
350  2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz;
351  double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc +
352  q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz;
353 
354  // q_mag is the quaternion that rotates the Global frame (North West Up) into
355  // the intermediary frame. q1_mag and q2_mag are defined as 0.
356  double gamma = lx*lx + ly*ly;
357  double beta = sqrt(gamma + lx*sqrt(gamma));
358  double q0_mag = beta / (sqrt(2.0 * gamma));
359  double q3_mag = ly / (sqrt(2.0) * beta);
360 
361  // The quaternion multiplication between q_acc and q_mag represents the
362  // quaternion, orientation of the Global frame wrt the local frame.
363  // q = q_acc times q_mag
364  quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc,
365  q0_mag, 0, 0, q3_mag,
366  q0_meas, q1_meas, q2_meas, q3_meas );
367  //q0_meas = q0_acc*q0_mag;
368  //q1_meas = q1_acc*q0_mag + q2_acc*q3_mag;
369  //q2_meas = q2_acc*q0_mag - q1_acc*q3_mag;
370  //q3_meas = q0_acc*q3_mag;
371 }
372 
373 
375  double ax, double ay, double az,
376  double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
377 {
378  // q_acc is the quaternion obtained from the acceleration vector representing
379  // the orientation of the Global frame wrt the Local frame with arbitrary yaw
380  // (intermediary frame). q3_acc is defined as 0.
381 
382  // Normalize acceleration vector.
383  normalizeVector(ax, ay, az);
384 
385  if (az >=0)
386  {
387  q0_meas = sqrt((az + 1) * 0.5);
388  q1_meas = -ay/(2.0 * q0_meas);
389  q2_meas = ax/(2.0 * q0_meas);
390  q3_meas = 0;
391  }
392  else
393  {
394  double X = sqrt((1 - az) * 0.5);
395  q0_meas = -ay/(2.0 * X);
396  q1_meas = X;
397  q2_meas = 0;
398  q3_meas = ax/(2.0 * X);
399  }
400 }
401 
403  double ax, double ay, double az,
404  double p0, double p1, double p2, double p3,
405  double& dq0, double& dq1, double& dq2, double& dq3)
406 {
407  // Normalize acceleration vector.
408  normalizeVector(ax, ay, az);
409 
410  // Acceleration reading rotated into the world frame by the inverse predicted
411  // quaternion (predicted gravity):
412  double gx, gy, gz;
413  rotateVectorByQuaternion(ax, ay, az,
414  p0, -p1, -p2, -p3,
415  gx, gy, gz);
416 
417  // Delta quaternion that rotates the predicted gravity into the real gravity:
418  dq0 = sqrt((gz + 1) * 0.5);
419  dq1 = -gy/(2.0 * dq0);
420  dq2 = gx/(2.0 * dq0);
421  dq3 = 0.0;
422 }
423 
425  double mx, double my, double mz,
426  double p0, double p1, double p2, double p3,
427  double& dq0, double& dq1, double& dq2, double& dq3)
428 {
429  // Magnetic reading rotated into the world frame by the inverse predicted
430  // quaternion:
431  double lx, ly, lz;
432  rotateVectorByQuaternion(mx, my, mz,
433  p0, -p1, -p2, -p3,
434  lx, ly, lz);
435 
436  // Delta quaternion that rotates the l so that it lies in the xz-plane
437  // (points north):
438  double gamma = lx*lx + ly*ly;
439  double beta = sqrt(gamma + lx*sqrt(gamma));
440  dq0 = beta / (sqrt(2.0 * gamma));
441  dq1 = 0.0;
442  dq2 = 0.0;
443  dq3 = ly / (sqrt(2.0) * beta);
444 }
445 
447  double& q0, double& q1, double& q2, double& q3) const
448 {
449  // Return the inverse of the state (state is fixed wrt body).
450  invertQuaternion(q0_, q1_, q2_, q3_, q0, q1, q2, q3);
451 }
452 
453 double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, double az)
454 {
455  double a_mag = sqrt(ax*ax + ay*ay + az*az);
456  double error = fabs(a_mag - kGravity)/kGravity;
457  double factor;
458  double error1 = 0.1;
459  double error2 = 0.2;
460  double m = 1.0/(error1 - error2);
461  double b = 1.0 - m*error1;
462  if (error < error1)
463  factor = 1.0;
464  else if (error < error2)
465  factor = m*error + b;
466  else
467  factor = 0.0;
468  //printf("FACTOR: %f \n", factor);
469  return factor*alpha;
470 }
471 
472 void normalizeVector(double& x, double& y, double& z)
473 {
474  double norm = sqrt(x*x + y*y + z*z);
475 
476  x /= norm;
477  y /= norm;
478  z /= norm;
479 }
480 
481 void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
482 {
483  double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
484  q0 /= norm;
485  q1 /= norm;
486  q2 /= norm;
487  q3 /= norm;
488 }
489 
491  double q0, double q1, double q2, double q3,
492  double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv)
493 {
494  // Assumes quaternion is normalized.
495  q0_inv = q0;
496  q1_inv = -q1;
497  q2_inv = -q2;
498  q3_inv = -q3;
499 }
500 
502  double gain,
503  double& dq0, double& dq1, double& dq2, double& dq3)
504 {
505  if (dq0 < 0.0)//0.9
506  {
507  // Slerp (Spherical linear interpolation):
508  double angle = acos(dq0);
509  double A = sin(angle*(1.0 - gain))/sin(angle);
510  double B = sin(angle * gain)/sin(angle);
511  dq0 = A + B * dq0;
512  dq1 = B * dq1;
513  dq2 = B * dq2;
514  dq3 = B * dq3;
515  }
516  else
517  {
518  // Lerp (Linear interpolation):
519  dq0 = (1.0 - gain) + gain * dq0;
520  dq1 = gain * dq1;
521  dq2 = gain * dq2;
522  dq3 = gain * dq3;
523  }
524 
525  normalizeQuaternion(dq0, dq1, dq2, dq3);
526 }
527 
529  double p0, double p1, double p2, double p3,
530  double q0, double q1, double q2, double q3,
531  double& r0, double& r1, double& r2, double& r3)
532 {
533  // r = p q
534  r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3;
535  r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2;
536  r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1;
537  r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0;
538 }
539 
541  double x, double y, double z,
542  double q0, double q1, double q2, double q3,
543  double& vx, double& vy, double& vz)
544 {
545  vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z;
546  vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z;
547  vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z;
548 }
549 
550 
551 } // namespace imu_tools
void setOrientation(double q0, double q1, double q2, double q3)
void setDoAdaptiveGain(bool do_adaptive_gain)
void rotateVectorByQuaternion(double x, double y, double z, double q0, double q1, double q2, double q3, double &vx, double &vy, double &vz)
static const double kAngularVelocityThreshold
bool setBiasAlpha(double bias_alpha)
void updateBiases(double ax, double ay, double az, double wx, double wy, double wz)
void normalizeQuaternion(double &q0, double &q1, double &q2, double &q3)
void quaternionMultiplication(double p0, double p1, double p2, double p3, double q0, double q1, double q2, double q3, double &r0, double &r1, double &r2, double &r3)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
void normalizeVector(double &x, double &y, double &z)
double getAdaptiveGain(double alpha, double ax, double ay, double az)
void getOrientation(double &q0, double &q1, double &q2, double &q3) const
void setDoBiasEstimation(bool do_bias_estimation)
static const double kDeltaAngularVelocityThreshold
void getMeasurement(double ax, double ay, double az, double &q0_meas, double &q1_meas, double &q2_meas, double &q3_meas)
void scaleQuaternion(double gain, double &dq0, double &dq1, double &dq2, double &dq3)
void update(double ax, double ay, double az, double wx, double wy, double wz, double dt)
bool checkState(double ax, double ay, double az, double wx, double wy, double wz) const
void getAccCorrection(double ax, double ay, double az, double p0, double p1, double p2, double p3, double &dq0, double &dq1, double &dq2, double &dq3)
void getPrediction(double wx, double wy, double wz, double dt, double &q0_pred, double &q1_pred, double &q2_pred, double &q3_pred) const
void getMagCorrection(double mx, double my, double mz, double p0, double p1, double p2, double p3, double &dq0, double &dq1, double &dq2, double &dq3)
void invertQuaternion(double q0, double q1, double q2, double q3, double &q0_inv, double &q1_inv, double &q2_inv, double &q3_inv)
static const double kAccelerationThreshold


imu_complementary_filter
Author(s): Roberto G. Valenti
autogenerated on Tue May 7 2019 03:16:52