ComplementaryFilter.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 
32 #include "ComplementaryFilter.h"
34 #include <cstdio>
35 #include <cmath>
36 #include <iostream>
37 
38 namespace rtabmap {
39 
40 const double ComplementaryFilter::kGravity = 9.81;
41 const double ComplementaryFilter::gamma_ = 0.01;
42 // Bias estimation steady state thresholds
46 
48  IMUFilter(parameters),
49  gain_acc_(Parameters::defaultImuFilterComplementaryGainAcc()),
50  bias_alpha_(Parameters::defaultImuFilterComplementaryBiasAlpha()),
51  do_bias_estimation_(Parameters::defaultImuFilterComplementaryDoBiasEstimation()),
52  do_adaptive_gain_(Parameters::defaultImuFilterComplementaryDoAdpativeGain()),
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 {
59  parseParameters(parameters);
60 }
61 
63 {
64  Parameters::parse(parameters, Parameters::kImuFilterComplementaryGainAcc(), gain_acc_);
65  Parameters::parse(parameters, Parameters::kImuFilterComplementaryBiasAlpha(), bias_alpha_);
66  Parameters::parse(parameters, Parameters::kImuFilterComplementaryDoBiasEstimation(), do_bias_estimation_);
67  Parameters::parse(parameters, Parameters::kImuFilterComplementaryDoAdpativeGain(), do_adaptive_gain_);
68 }
69 
70 void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation)
71 {
72  do_bias_estimation_ = do_bias_estimation;
73 }
74 
76 {
77  return do_bias_estimation_;
78 }
79 
80 void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain)
81 {
82  do_adaptive_gain_ = do_adaptive_gain;
83 }
84 
86 {
87  return do_adaptive_gain_;
88 }
89 
91 {
92  if (gain >= 0 && gain <= 1.0)
93  {
94  gain_acc_ = gain;
95  return true;
96  }
97  else
98  return false;
99 }
100 
102 {
103  return gain_acc_;
104 }
105 
107 {
108  return steady_state_;
109 }
110 
111 bool ComplementaryFilter::setBiasAlpha(double bias_alpha)
112 {
113  if (bias_alpha >= 0 && bias_alpha <= 1.0)
114  {
115  bias_alpha_ = bias_alpha;
116  return true;
117  }
118  else
119  return false;
120 }
121 
123 {
124  return bias_alpha_;
125 }
126 
128  double qx, double qy, double qz, double qw)
129 {
130  // Set the state to inverse (state is fixed wrt body).
131  invertQuaternion(qw, qx, qy, qz, q0_, q1_, q2_, q3_);
132 
133  wx_prev_=0;
134  wy_prev_=0;
135  wz_prev_=0;
136  wx_bias_=0;
137  wy_bias_=0;
138  wz_bias_=0;
139  initialized_ = false;
140  steady_state_= false;
141 }
142 
143 
145 {
146  return wx_bias_;
147 }
148 
150 {
151  return wy_bias_;
152 }
153 
155 {
156  return wz_bias_;
157 }
158 
160  double gx, double gy, double gz,
161  double ax, double ay, double az,
162  double dt)
163 {
164  if (!initialized_)
165  {
166  // First time - ignore prediction:
167  getMeasurement(ax, ay, az,
168  q0_, q1_, q2_, q3_);
169  initialized_ = true;
170  return;
171  }
172 
173  if(dt <= 0.0)
174  {
175  UWARN("dt=%f <=0.0, orientation will not be updated!", dt);
176  return;
177  }
178 
179  // Bias estimation.
181  updateBiases(ax, ay, az, gx, gy, gz);
182 
183  // Prediction.
184  double q0_pred, q1_pred, q2_pred, q3_pred;
185  getPrediction(gx, gy, gz, dt,
186  q0_pred, q1_pred, q2_pred, q3_pred);
187 
188  // Correction (from acc):
189  // q_ = q_pred * [(1-gain) * qI + gain * dq_acc]
190  // where qI = identity quaternion
191  double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
192  getAccCorrection(ax, ay, az,
193  q0_pred, q1_pred, q2_pred, q3_pred,
194  dq0_acc, dq1_acc, dq2_acc, dq3_acc);
195 
196  double gain;
197  if (do_adaptive_gain_)
198  {
199  gain = getAdaptiveGain(gain_acc_, ax, ay, az);
200 
201  }
202  else
203  {
204  gain = gain_acc_;
205 
206  }
207 
208  scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
209 
210  quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
211  dq0_acc, dq1_acc, dq2_acc, dq3_acc,
212  q0_, q1_, q2_, q3_);
213 
215 }
216 
217 bool ComplementaryFilter::checkState(double ax, double ay, double az,
218  double wx, double wy, double wz) const
219 {
220  double acc_magnitude = sqrt(ax*ax + ay*ay + az*az);
221  if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold)
222  return false;
223 
224  if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold ||
227  return false;
228 
229  if (fabs(wx - wx_bias_) > kAngularVelocityThreshold ||
230  fabs(wy - wy_bias_) > kAngularVelocityThreshold ||
231  fabs(wz - wz_bias_) > kAngularVelocityThreshold)
232  return false;
233 
234  return true;
235 }
236 
237 void ComplementaryFilter::updateBiases(double ax, double ay, double az,
238  double wx, double wy, double wz)
239 {
240  steady_state_ = checkState(ax, ay, az, wx, wy, wz);
241 
242  if (steady_state_)
243  {
244  wx_bias_ += bias_alpha_ * (wx - wx_bias_);
245  wy_bias_ += bias_alpha_ * (wy - wy_bias_);
246  wz_bias_ += bias_alpha_ * (wz - wz_bias_);
247  }
248 
249  wx_prev_ = wx;
250  wy_prev_ = wy;
251  wz_prev_ = wz;
252 }
253 
255  double wx, double wy, double wz, double dt,
256  double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const
257 {
258  double wx_unb = wx - wx_bias_;
259  double wy_unb = wy - wy_bias_;
260  double wz_unb = wz - wz_bias_;
261 
262  q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_);
263  q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_);
264  q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_);
265  q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_);
266 
267  normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred);
268 }
269 
271  double ax, double ay, double az,
272  double mx, double my, double mz,
273  double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
274 {
275  // q_acc is the quaternion obtained from the acceleration vector representing
276  // the orientation of the Global frame wrt the Local frame with arbitrary yaw
277  // (intermediary frame). q3_acc is defined as 0.
278  double q0_acc, q1_acc, q2_acc, q3_acc;
279 
280  // Normalize acceleration vector.
281  normalizeVector(ax, ay, az);
282  if (az >=0)
283  {
284  q0_acc = sqrt((az + 1) * 0.5);
285  q1_acc = -ay/(2.0 * q0_acc);
286  q2_acc = ax/(2.0 * q0_acc);
287  q3_acc = 0;
288  }
289  else
290  {
291  double X = sqrt((1 - az) * 0.5);
292  q0_acc = -ay/(2.0 * X);
293  q1_acc = X;
294  q2_acc = 0;
295  q3_acc = ax/(2.0 * X);
296  }
297 
298  // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary
299  // frame by the inverse of q_acc.
300  // l = R(q_acc)^-1 m
301  double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx +
302  2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz;
303  double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc +
304  q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz;
305 
306  // q_mag is the quaternion that rotates the Global frame (North West Up) into
307  // the intermediary frame. q1_mag and q2_mag are defined as 0.
308  double gamma = lx*lx + ly*ly;
309  double beta = sqrt(gamma + lx*sqrt(gamma));
310  double q0_mag = beta / (sqrt(2.0 * gamma));
311  double q3_mag = ly / (sqrt(2.0) * beta);
312 
313  // The quaternion multiplication between q_acc and q_mag represents the
314  // quaternion, orientation of the Global frame wrt the local frame.
315  // q = q_acc times q_mag
316  quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc,
317  q0_mag, 0, 0, q3_mag,
318  q0_meas, q1_meas, q2_meas, q3_meas );
319  //q0_meas = q0_acc*q0_mag;
320  //q1_meas = q1_acc*q0_mag + q2_acc*q3_mag;
321  //q2_meas = q2_acc*q0_mag - q1_acc*q3_mag;
322  //q3_meas = q0_acc*q3_mag;
323 }
324 
325 
327  double ax, double ay, double az,
328  double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
329 {
330  // q_acc is the quaternion obtained from the acceleration vector representing
331  // the orientation of the Global frame wrt the Local frame with arbitrary yaw
332  // (intermediary frame). q3_acc is defined as 0.
333 
334  // Normalize acceleration vector.
335  normalizeVector(ax, ay, az);
336 
337  if (az >=0)
338  {
339  q0_meas = sqrt((az + 1) * 0.5);
340  q1_meas = -ay/(2.0 * q0_meas);
341  q2_meas = ax/(2.0 * q0_meas);
342  q3_meas = 0;
343  }
344  else
345  {
346  double X = sqrt((1 - az) * 0.5);
347  q0_meas = -ay/(2.0 * X);
348  q1_meas = X;
349  q2_meas = 0;
350  q3_meas = ax/(2.0 * X);
351  }
352 }
353 
355  double ax, double ay, double az,
356  double p0, double p1, double p2, double p3,
357  double& dq0, double& dq1, double& dq2, double& dq3)
358 {
359  // Normalize acceleration vector.
360  normalizeVector(ax, ay, az);
361 
362  // Acceleration reading rotated into the world frame by the inverse predicted
363  // quaternion (predicted gravity):
364  double gx, gy, gz;
365  rotateVectorByQuaternion(ax, ay, az,
366  p0, -p1, -p2, -p3,
367  gx, gy, gz);
368 
369  // Delta quaternion that rotates the predicted gravity into the real gravity:
370  dq0 = sqrt((gz + 1) * 0.5);
371  dq1 = -gy/(2.0 * dq0);
372  dq2 = gx/(2.0 * dq0);
373  dq3 = 0.0;
374 }
375 
377  double& qx, double& qy, double& qz, double& qw) const
378 {
379  // Return the inverse of the state (state is fixed wrt body).
380  invertQuaternion(q0_, q1_, q2_, q3_, qw,qx,qy,qz);
381 }
382 
383 double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, double az)
384 {
385  double a_mag = sqrt(ax*ax + ay*ay + az*az);
386  double error = fabs(a_mag - kGravity)/kGravity;
387  double factor;
388  double error1 = 0.1;
389  double error2 = 0.2;
390  double m = 1.0/(error1 - error2);
391  double b = 1.0 - m*error1;
392  if (error < error1)
393  factor = 1.0;
394  else if (error < error2)
395  factor = m*error + b;
396  else
397  factor = 0.0;
398  //printf("FACTOR: %f \n", factor);
399  return factor*alpha;
400 }
401 
402 void normalizeVector(double& x, double& y, double& z)
403 {
404  double norm = sqrt(x*x + y*y + z*z);
405 
406  x /= norm;
407  y /= norm;
408  z /= norm;
409 }
410 
411 void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
412 {
413  double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
414  q0 /= norm;
415  q1 /= norm;
416  q2 /= norm;
417  q3 /= norm;
418 }
419 
421  double q0, double q1, double q2, double q3,
422  double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv)
423 {
424  // Assumes quaternion is normalized.
425  q0_inv = q0;
426  q1_inv = -q1;
427  q2_inv = -q2;
428  q3_inv = -q3;
429 }
430 
432  double gain,
433  double& dq0, double& dq1, double& dq2, double& dq3)
434 {
435  if (dq0 < 0.0)//0.9
436  {
437  // Slerp (Spherical linear interpolation):
438  double angle = acos(dq0);
439  double A = sin(angle*(1.0 - gain))/sin(angle);
440  double B = sin(angle * gain)/sin(angle);
441  dq0 = A + B * dq0;
442  dq1 = B * dq1;
443  dq2 = B * dq2;
444  dq3 = B * dq3;
445  }
446  else
447  {
448  // Lerp (Linear interpolation):
449  dq0 = (1.0 - gain) + gain * dq0;
450  dq1 = gain * dq1;
451  dq2 = gain * dq2;
452  dq3 = gain * dq3;
453  }
454 
455  normalizeQuaternion(dq0, dq1, dq2, dq3);
456 }
457 
459  double p0, double p1, double p2, double p3,
460  double q0, double q1, double q2, double q3,
461  double& r0, double& r1, double& r2, double& r3)
462 {
463  // r = p q
464  r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3;
465  r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2;
466  r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1;
467  r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0;
468 }
469 
471  double x, double y, double z,
472  double q0, double q1, double q2, double q3,
473  double& vx, double& vy, double& vz)
474 {
475  vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z;
476  vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z;
477  vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z;
478 }
479 
480 
481 } // namespace rtabmap
482 
void updateBiases(double ax, double ay, double az, double wx, double wy, double wz)
virtual void reset(double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0)
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
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
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
void normalizeQuaternion(double &q0, double &q1, double &q2, double &q3)
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
virtual void getOrientation(double &qx, double &qy, double &qz, double &qw) const
void getMeasurement(double ax, double ay, double az, double &q0_meas, double &q1_meas, double &q2_meas, double &q3_meas)
virtual void parseParameters(const ParametersMap &parameters)
static const double kDeltaAngularVelocityThreshold
GLM_FUNC_DECL genType sin(genType const &angle)
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)
void setDoAdaptiveGain(bool do_adaptive_gain)
bool checkState(double ax, double ay, double az, double wx, double wy, double wz) const
bool setBiasAlpha(double bias_alpha)
void scaleQuaternion(double gain, double &dq0, double &dq1, double &dq2, double &dq3)
#define false
Definition: ConvertUTF.c:56
void normalizeVector(double &x, double &y, double &z)
virtual void updateImpl(double gx, double gy, double gz, double ax, double ay, double az, double dt)
ULogger class and convenient macros.
GLM_FUNC_DECL genType acos(genType const &x)
#define UWARN(...)
ComplementaryFilter(const ParametersMap &parameters=ParametersMap())
void rotateVectorByQuaternion(double x, double y, double z, double q0, double q1, double q2, double q3, double &vx, double &vy, double &vz)
double getAdaptiveGain(double alpha, double ax, double ay, double az)
void setDoBiasEstimation(bool do_bias_estimation)
void getPrediction(double wx, double wy, double wz, double dt, double &q0_pred, double &q1_pred, double &q2_pred, double &q3_pred) 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)
static const double kAngularVelocityThreshold


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28