MadgwickFilter.cpp
Go to the documentation of this file.
1 
2 /*
3  * Copyright (C) 2010, CCNY Robotics Lab
4  * Ivan Dryanovski <ivan.dryanovski@gmail.com>
5  *
6  * http://robotics.ccny.cuny.edu
7  *
8  * Based on implementation of Madgwick's IMU and AHRS algorithms.
9  * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
10  *
11  *
12  * This program is free software: you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation, either version 3 of the License, or
15  * (at your option) any later version.
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU General Public License for more details.
21  *
22  * You should have received a copy of the GNU General Public License
23  * along with this program. If not, see <http://www.gnu.org/licenses/>.
24  */
25 
26 #include "MadgwickFilter.h"
28 
29 namespace rtabmap {
30 
31 // Fast inverse square-root
32 // See: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Reciprocal_of_the_square_root
33 static inline float invSqrt(float x)
34 {
35  float xhalf = 0.5f * x;
36  union
37  {
38  float x;
39  int i;
40  } u;
41  u.x = x;
42  u.i = 0x5f3759df - (u.i >> 1);
43  /* The next line can be repeated any number of times to increase accuracy */
44  u.x = u.x * (1.5f - xhalf * u.x * u.x);
45  return u.x;
46 }
47 
48 template<typename T>
49 static inline void normalizeVectorOpt(T& vx, T& vy, T& vz)
50 {
51  T recipNorm = invSqrt (vx * vx + vy * vy + vz * vz);
52  vx *= recipNorm;
53  vy *= recipNorm;
54  vz *= recipNorm;
55 }
56 
57 template<typename T>
58 static inline void normalizeQuaternion(T& q0, T& q1, T& q2, T& q3)
59 {
60  T recipNorm = invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
61  q0 *= recipNorm;
62  q1 *= recipNorm;
63  q2 *= recipNorm;
64  q3 *= recipNorm;
65 }
66 
67 static inline void rotateAndScaleVector(
68  float q0, float q1, float q2, float q3,
69  float _2dx, float _2dy, float _2dz,
70  float& rx, float& ry, float& rz) {
71 
72  // result is half as long as input
73  rx = _2dx * (0.5f - q2 * q2 - q3 * q3)
74  + _2dy * (q0 * q3 + q1 * q2)
75  + _2dz * (q1 * q3 - q0 * q2);
76  ry = _2dx * (q1 * q2 - q0 * q3)
77  + _2dy * (0.5f - q1 * q1 - q3 * q3)
78  + _2dz * (q0 * q1 + q2 * q3);
79  rz = _2dx * (q0 * q2 + q1 * q3)
80  + _2dy * (q2 * q3 - q0 * q1)
81  + _2dz * (0.5f - q1 * q1 - q2 * q2);
82 }
83 
84 static inline void orientationChangeFromGyro(
85  float q0, float q1, float q2, float q3,
86  float gx, float gy, float gz,
87  float& qDot1, float& qDot2, float& qDot3, float& qDot4)
88 {
89  // Rate of change of quaternion from gyroscope
90  // See EQ 12
91  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
92  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
93  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
94  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
95 }
96 
97 static inline void addGradientDescentStep(
98  float q0, float q1, float q2, float q3,
99  float _2dx, float _2dy, float _2dz,
100  float mx, float my, float mz,
101  float& s0, float& s1, float& s2, float& s3)
102 {
103  float f0, f1, f2;
104 
105  // Gradient decent algorithm corrective step
106  // EQ 15, 21
107  rotateAndScaleVector(q0,q1,q2,q3, _2dx, _2dy, _2dz, f0, f1, f2);
108 
109  f0 -= mx;
110  f1 -= my;
111  f2 -= mz;
112 
113 
114  // EQ 22, 34
115  // Jt * f
116  s0 += (_2dy * q3 - _2dz * q2) * f0
117  + (-_2dx * q3 + _2dz * q1) * f1
118  + (_2dx * q2 - _2dy * q1) * f2;
119  s1 += (_2dy * q2 + _2dz * q3) * f0
120  + (_2dx * q2 - 2.0f * _2dy * q1 + _2dz * q0) * f1
121  + (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2;
122  s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0
123  + (_2dx * q1 + _2dz * q3) * f1
124  + (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2;
125  s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0
126  + (-_2dx * q0 - 2.0f * _2dy * q3 + _2dz * q2) * f1
127  + (_2dx * q1 + _2dy * q2) * f2;
128 }
129 
130 template<typename T>
131 static inline void crossProduct(
132  T ax, T ay, T az,
133  T bx, T by, T bz,
134  T& rx, T& ry, T& rz) {
135  rx = ay*bz - az*by;
136  ry = az*bx - ax*bz;
137  rz = ax*by - ay*bx;
138 }
139 
140 template<typename T>
141 static inline T normalizeVector(T& vx, T& vy, T& vz) {
142  T norm = sqrt(vx*vx + vy*vy + vz*vz);
143  T inv = 1.0 / norm;
144  vx *= inv;
145  vy *= inv;
146  vz *= inv;
147  return norm;
148 
149 }
150 
151 static inline bool computeOrientation(
152  Eigen::Vector3f A,
153  Eigen::Vector3f E,
155 
156  float Hx, Hy, Hz;
157  float Mx, My, Mz;
158  float normH;
159 
160  // A: pointing up
161  float Ax = A[0], Ay = A[1], Az = A[2];
162 
163  // E: pointing down/north
164  float Ex = E[0], Ey = E[1], Ez = E[2];
165 
166  // H: vector horizontal, pointing east
167  // H = E x A
168  crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz);
169 
170  // normalize H
171  normH = normalizeVector(Hx, Hy, Hz);
172  if (normH < 1E-7) {
173  // device is close to free fall (or in space?), or close to
174  // magnetic north pole.
175  // mag in T => Threshold 1E-7, typical values are > 1E-5.
176  return false;
177  }
178 
179  // normalize A
180  normalizeVector(Ax, Ay, Az);
181 
182  // M: vector horizontal, pointing north
183  // M = A x H
184  crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz);
185 
186  // Create matrix for basis transformation
187  Eigen::Matrix3f R;
188 
189  //case WorldFrame::ENU:
190  // vector space world W:
191  // Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up
192  // vector space local L:
193  // Basis: H, M , A
194  // W(1,0,0) => L(H)
195  // W(0,1,0) => L(M)
196  // W(0,0,1) => L(A)
197 
198  // R: Transform Matrix local => world equals basis of L, because basis of W is I
199  R(0,0) = Hx; R(0,1) = Mx; R(0,2) = Ax;
200  R(1,0) = Hy; R(1,1) = My; R(1,2) = Ay;
201  R(2,0) = Hz; R(2,1) = Mz; R(2,2) = Az;
202 
203  // Matrix.getRotation assumes vector rotation, but we're using
204  // coordinate systems. Thus negate rotation angle (inverse).
206  orientation = q.inverse();
207  return true;
208 }
209 
210 
211 static inline bool computeOrientation(
212  Eigen::Vector3f A,
214 
215  // This implementation could be optimized regarding speed.
216 
217  // magnetic Field E must not be parallel to A,
218  // choose an arbitrary orthogonal vector
219  Eigen::Vector3f E;
220  if (fabs(A[2]) > 0.1) {
221  E[0] = 0.0;
222  E[1] = A[2];
223  E[2] = -A[1];
224  } else if (fabs(A[0]) > 0.1 || fabs(A[1]) > 0.1) {
225  E[0] = A[1];
226  E[1] = A[0];
227  E[2] = 0.0;
228  } else {
229  // free fall
230  return false;
231  }
232 
233  return computeOrientation(A, E, orientation);
234 }
235 
237  IMUFilter(parameters),
238  q0(1.0), q1(0.0), q2(0.0), q3(0.0),
239  w_bx_(0.0), w_by_(0.0), w_bz_(0.0),
240  initialized_(false),
241  gain_ (Parameters::defaultImuFilterMadgwickGain()),
242  zeta_ (Parameters::defaultImuFilterMadgwickZeta())
243 {
244  parseParameters(parameters);
245 }
246 
248 {
249  Parameters::parse(parameters, Parameters::kImuFilterMadgwickGain(), gain_);
250  Parameters::parse(parameters, Parameters::kImuFilterMadgwickZeta(), zeta_);
251 }
252 
258 {
259  gain_ = gain;
260 }
261 
266 {
267  zeta_ = zeta;
268 }
269 
270 void MadgwickFilter::getOrientation(double & qx, double & qy, double & qz, double & qw) const
271 {
272  qx = this->q1;
273  qy = this->q2;
274  qz = this->q3;
275  qw = this->q0;
276 
277  // perform precise normalization of the output, using 1/sqrt()
278  // instead of the fast invSqrt() approximation. Without this,
279  // TF2 complains that the quaternion is not normalized.
280  double recipNorm = 1 / sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
281  qx *= recipNorm;
282  qy *= recipNorm;
283  qz *= recipNorm;
284  qw *= recipNorm;
285 }
286 
287 void MadgwickFilter::reset(double qx, double qy, double qz, double qw)
288 {
289  this->q0 = qw;
290  this->q1 = qx;
291  this->q2 = qy;
292  this->q3 = qz;
293 
294  w_bx_ = 0;
295  w_by_ = 0;
296  w_bz_ = 0;
297 
298  initialized_ = false;
299 }
300 
302  double gx, double gy, double gz,
303  double ax, double ay, double az,
304  double dt)
305 {
306  if(!initialized_)
307  {
309  Eigen::Vector3f A;
310  A[0] = ax;
311  A[1] = ay;
312  A[2] = az;
314  {
316  initialized_ = true;
317  }
318  return;
319  }
320 
321  float s0, s1, s2, s3;
322  float qDot1, qDot2, qDot3, qDot4;
323 
324  // Rate of change of quaternion from gyroscope
325  orientationChangeFromGyro (q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
326 
327  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
328  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
329  {
330  // Normalise accelerometer measurement
331  normalizeVectorOpt(ax, ay, az);
332 
333  // Gradient decent algorithm corrective step
334  s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
335  //case WorldFrame::ENU:
336  // Gravity: [0, 0, 1]
337  addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
338 
339  normalizeQuaternion(s0, s1, s2, s3);
340 
341  // Apply feedback step
342  qDot1 -= gain_ * s0;
343  qDot2 -= gain_ * s1;
344  qDot3 -= gain_ * s2;
345  qDot4 -= gain_ * s3;
346  }
347 
348  // Integrate rate of change of quaternion to yield quaternion
349  if(dt <= 0.0)
350  {
351  UWARN("dt=%f <=0.0, orientation will not be updated!", dt);
352  return;
353  }
354  q0 += qDot1 * dt;
355  q1 += qDot2 * dt;
356  q2 += qDot3 * dt;
357  q3 += qDot4 * dt;
358 
359  //printf("%fs %f %f %f %f\n", dt, q0, q1, q2, q3);
360 
361  // Normalise quaternion
363 }
364 
365 }
f2
const T1 const T2 & f2
rtabmap::MadgwickFilter::reset
virtual void reset(double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0)
Definition: MadgwickFilter.cpp:287
rtabmap::MadgwickFilter::getOrientation
virtual void getOrientation(double &qx, double &qy, double &qz, double &qw) const
Definition: MadgwickFilter.cpp:270
rtabmap::orientationChangeFromGyro
static void orientationChangeFromGyro(float q0, float q1, float q2, float q3, float gx, float gy, float gz, float &qDot1, float &qDot2, float &qDot3, float &qDot4)
Definition: MadgwickFilter.cpp:84
rtabmap::MadgwickFilter::q1
double q1
Definition: MadgwickFilter.h:43
MadgwickFilter.h
rtabmap::MadgwickFilter::q3
double q3
Definition: MadgwickFilter.h:43
rtabmap::MadgwickFilter::q2
double q2
Definition: MadgwickFilter.h:43
dt
const double dt
E
int E
rtabmap::rotateAndScaleVector
static void rotateAndScaleVector(float q0, float q1, float q2, float q3, float _2dx, float _2dy, float _2dz, float &rx, float &ry, float &rz)
Definition: MadgwickFilter.cpp:67
rtabmap::MadgwickFilter::w_bz_
float w_bz_
Definition: MadgwickFilter.h:44
rtabmap::MadgwickFilter::updateImpl
void updateImpl(double gx, double gy, double gz, double ax, double ay, double az, double dt)
Definition: MadgwickFilter.cpp:301
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
zeta
const CwiseBinaryOp< internal::scalar_zeta_op< Scalar >, const Derived, const DerivedQ > zeta(const EIGEN_CURRENT_STORAGE_BASE_CLASS< DerivedQ > &q) const
rtabmap::MadgwickFilter::w_by_
float w_by_
Definition: MadgwickFilter.h:44
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::MadgwickFilter::gain_
double gain_
Definition: MadgwickFilter.h:48
fabs
Real fabs(const Real &a)
rtabmap::MadgwickFilter::zeta_
double zeta_
Definition: MadgwickFilter.h:49
rtabmap::IMUFilter
Definition: IMUFilter.h:37
A
vy
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
rtabmap::addGradientDescentStep
static void addGradientDescentStep(float q0, float q1, float q2, float q3, float _2dx, float _2dy, float _2dz, float mx, float my, float mz, float &s0, float &s1, float &s2, float &s3)
Definition: MadgwickFilter.cpp:97
rtabmap::normalizeQuaternion
void normalizeQuaternion(double &q0, double &q1, double &q2, double &q3)
Definition: ComplementaryFilter.cpp:411
q
EIGEN_DEVICE_FUNC const Scalar & q
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::MadgwickFilter::w_bx_
float w_bx_
Definition: MadgwickFilter.h:44
glm::orientation
GLM_FUNC_DECL detail::tmat4x4< T, P > orientation(detail::tvec3< T, P > const &Normal, detail::tvec3< T, P > const &Up)
rtabmap::normalizeVector
void normalizeVector(double &x, double &y, double &z)
Definition: ComplementaryFilter.cpp:402
x
x
Eigen::Triplet< double >
rtabmap::invSqrt
static float invSqrt(float x)
Definition: MadgwickFilter.cpp:33
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::MadgwickFilter::initialized_
bool initialized_
Definition: MadgwickFilter.h:45
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
qz
RealQZ< MatrixXf > qz(4)
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
rtabmap::MadgwickFilter::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: MadgwickFilter.cpp:247
rtabmap::computeOrientation
static bool computeOrientation(Eigen::Vector3f A, Eigen::Vector3f E, Eigen::Quaternionf &orientation)
Definition: MadgwickFilter.cpp:151
false
#define false
Definition: ConvertUTF.c:56
rtabmap::MadgwickFilter::q0
double q0
Definition: MadgwickFilter.h:43
rtabmap::MadgwickFilter::setDriftBiasGain
void setDriftBiasGain(double zeta)
Definition: MadgwickFilter.cpp:265
f1
const T1 & f1
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::crossProduct
static void crossProduct(T ax, T ay, T az, T bx, T by, T bz, T &rx, T &ry, T &rz)
Definition: MadgwickFilter.cpp:131
rtabmap::MadgwickFilter::setAlgorithmGain
void setAlgorithmGain(double gain)
Definition: MadgwickFilter.cpp:257
i
int i
vx
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
rtabmap::MadgwickFilter::MadgwickFilter
MadgwickFilter(const ParametersMap &parameters=ParametersMap())
Definition: MadgwickFilter.cpp:236
rtabmap::normalizeVectorOpt
static void normalizeVectorOpt(T &vx, T &vy, T &vz)
Definition: MadgwickFilter.cpp:49
R
R


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11