imu_filter.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010, CCNY Robotics Lab
3  * Ivan Dryanovski <ivan.dryanovski@gmail.com>
4  *
5  * http://robotics.ccny.cuny.edu
6  *
7  * Based on implementation of Madgwick's IMU and AHRS algorithms.
8  * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
9  *
10  *
11  * This program is free software: you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation, either version 3 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program. If not, see <http://www.gnu.org/licenses/>.
23  */
24 
25 #include <cmath>
27 
28 // Fast inverse square-root
29 // See: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Reciprocal_of_the_square_root
30 static float invSqrt(float x)
31 {
32  float xhalf = 0.5f * x;
33  union
34  {
35  float x;
36  int i;
37  } u;
38  u.x = x;
39  u.i = 0x5f3759df - (u.i >> 1);
40  /* The next line can be repeated any number of times to increase accuracy */
41  u.x = u.x * (1.5f - xhalf * u.x * u.x);
42  return u.x;
43 }
44 
45 template<typename T>
46 static inline void normalizeVector(T& vx, T& vy, T& vz)
47 {
48  T recipNorm = invSqrt (vx * vx + vy * vy + vz * vz);
49  vx *= recipNorm;
50  vy *= recipNorm;
51  vz *= recipNorm;
52 }
53 
54 template<typename T>
55 static inline void normalizeQuaternion(T& q0, T& q1, T& q2, T& q3)
56 {
57  T recipNorm = invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
58  q0 *= recipNorm;
59  q1 *= recipNorm;
60  q2 *= recipNorm;
61  q3 *= recipNorm;
62 }
63 
64 static inline void rotateAndScaleVector(
65  float q0, float q1, float q2, float q3,
66  float _2dx, float _2dy, float _2dz,
67  float& rx, float& ry, float& rz) {
68 
69  // result is half as long as input
70  rx = _2dx * (0.5f - q2 * q2 - q3 * q3)
71  + _2dy * (q0 * q3 + q1 * q2)
72  + _2dz * (q1 * q3 - q0 * q2);
73  ry = _2dx * (q1 * q2 - q0 * q3)
74  + _2dy * (0.5f - q1 * q1 - q3 * q3)
75  + _2dz * (q0 * q1 + q2 * q3);
76  rz = _2dx * (q0 * q2 + q1 * q3)
77  + _2dy * (q2 * q3 - q0 * q1)
78  + _2dz * (0.5f - q1 * q1 - q2 * q2);
79 }
80 
81 
82 static inline void compensateGyroDrift(
83  float q0, float q1, float q2, float q3,
84  float s0, float s1, float s2, float s3,
85  float dt, float zeta,
86  float& w_bx, float& w_by, float& w_bz,
87  float& gx, float& gy, float& gz)
88 {
89  // w_err = 2 q x s
90  float w_err_x = 2.0f * q0 * s1 - 2.0f * q1 * s0 - 2.0f * q2 * s3 + 2.0f * q3 * s2;
91  float w_err_y = 2.0f * q0 * s2 + 2.0f * q1 * s3 - 2.0f * q2 * s0 - 2.0f * q3 * s1;
92  float w_err_z = 2.0f * q0 * s3 - 2.0f * q1 * s2 + 2.0f * q2 * s1 - 2.0f * q3 * s0;
93 
94  w_bx += w_err_x * dt * zeta;
95  w_by += w_err_y * dt * zeta;
96  w_bz += w_err_z * dt * zeta;
97 
98  gx -= w_bx;
99  gy -= w_by;
100  gz -= w_bz;
101 }
102 
103 static inline void orientationChangeFromGyro(
104  float q0, float q1, float q2, float q3,
105  float gx, float gy, float gz,
106  float& qDot1, float& qDot2, float& qDot3, float& qDot4)
107 {
108  // Rate of change of quaternion from gyroscope
109  // See EQ 12
110  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
111  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
112  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
113  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
114 }
115 
116 static inline void addGradientDescentStep(
117  float q0, float q1, float q2, float q3,
118  float _2dx, float _2dy, float _2dz,
119  float mx, float my, float mz,
120  float& s0, float& s1, float& s2, float& s3)
121 {
122  float f0, f1, f2;
123 
124  // Gradient decent algorithm corrective step
125  // EQ 15, 21
126  rotateAndScaleVector(q0,q1,q2,q3, _2dx, _2dy, _2dz, f0, f1, f2);
127 
128  f0 -= mx;
129  f1 -= my;
130  f2 -= mz;
131 
132 
133  // EQ 22, 34
134  // Jt * f
135  s0 += (_2dy * q3 - _2dz * q2) * f0
136  + (-_2dx * q3 + _2dz * q1) * f1
137  + (_2dx * q2 - _2dy * q1) * f2;
138  s1 += (_2dy * q2 + _2dz * q3) * f0
139  + (_2dx * q2 - 2.0f * _2dy * q1 + _2dz * q0) * f1
140  + (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2;
141  s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0
142  + (_2dx * q1 + _2dz * q3) * f1
143  + (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2;
144  s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0
145  + (-_2dx * q0 - 2.0f * _2dy * q3 + _2dz * q2) * f1
146  + (_2dx * q1 + _2dy * q2) * f2;
147 }
148 
149 static inline void compensateMagneticDistortion(
150  float q0, float q1, float q2, float q3,
151  float mx, float my, float mz,
152  float& _2bxy, float& _2bz)
153 {
154  float hx, hy, hz;
155  // Reference direction of Earth's magnetic field (See EQ 46)
156  rotateAndScaleVector(q0, -q1, -q2, -q3, mx, my, mz, hx, hy, hz);
157 
158  _2bxy = 4.0f * sqrt (hx * hx + hy * hy);
159  _2bz = 4.0f * hz;
160 
161 }
162 
163 
165  gain_ (0.0), zeta_ (0.0), world_frame_(WorldFrame::ENU),
166  q0(1.0), q1(0.0), q2(0.0), q3(0.0),
167  w_bx_(0.0), w_by_(0.0), w_bz_(0.0)
168 {
169 }
170 
172 {
173 }
174 
176  float gx, float gy, float gz,
177  float ax, float ay, float az,
178  float mx, float my, float mz,
179  float dt)
180 {
181  float s0, s1, s2, s3;
182  float qDot1, qDot2, qDot3, qDot4;
183  float _2bz, _2bxy;
184 
185  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
186  if (!std::isfinite(mx) || !std::isfinite(my) || !std::isfinite(mz))
187  {
188  madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);
189  return;
190  }
191 
192  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
193  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
194  {
195  // Normalise accelerometer measurement
196  normalizeVector(ax, ay, az);
197 
198  // Normalise magnetometer measurement
199  normalizeVector(mx, my, mz);
200 
201  // Compensate for magnetic distortion
202  compensateMagneticDistortion(q0, q1, q2, q3, mx, my, mz, _2bxy, _2bz);
203 
204  // Gradient decent algorithm corrective step
205  s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
206  switch (world_frame_) {
207  case WorldFrame::NED:
208  // Gravity: [0, 0, -1]
209  addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
210 
211  // Earth magnetic field: = [bxy, 0, bz]
212  addGradientDescentStep(q0,q1,q2,q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
213  break;
214  case WorldFrame::NWU:
215  // Gravity: [0, 0, 1]
216  addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
217 
218  // Earth magnetic field: = [bxy, 0, bz]
219  addGradientDescentStep(q0,q1,q2,q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
220  break;
221  default:
222  case WorldFrame::ENU:
223  // Gravity: [0, 0, 1]
224  addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
225 
226  // Earth magnetic field: = [0, bxy, bz]
227  addGradientDescentStep(q0, q1, q2, q3, 0.0, _2bxy, _2bz, mx, my, mz, s0, s1, s2, s3);
228  break;
229  }
230  normalizeQuaternion(s0, s1, s2, s3);
231 
232  // compute gyro drift bias
233  compensateGyroDrift(q0, q1, q2, q3, s0, s1, s2, s3, dt, zeta_, w_bx_, w_by_, w_bz_, gx, gy, gz);
234 
235  orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
236 
237  // Apply feedback step
238  qDot1 -= gain_ * s0;
239  qDot2 -= gain_ * s1;
240  qDot3 -= gain_ * s2;
241  qDot4 -= gain_ * s3;
242  }
243  else
244  {
245  orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
246  }
247 
248  // Integrate rate of change of quaternion to yield quaternion
249  q0 += qDot1 * dt;
250  q1 += qDot2 * dt;
251  q2 += qDot3 * dt;
252  q3 += qDot4 * dt;
253 
254  // Normalise quaternion
256 }
257 
259  float gx, float gy, float gz,
260  float ax, float ay, float az,
261  float dt)
262 {
263  float s0, s1, s2, s3;
264  float qDot1, qDot2, qDot3, qDot4;
265 
266  // Rate of change of quaternion from gyroscope
267  orientationChangeFromGyro (q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
268 
269  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
270  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
271  {
272  // Normalise accelerometer measurement
273  normalizeVector(ax, ay, az);
274 
275  // Gradient decent algorithm corrective step
276  s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
277  switch (world_frame_) {
278  case WorldFrame::NED:
279  // Gravity: [0, 0, -1]
280  addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
281  break;
282  case WorldFrame::NWU:
283  // Gravity: [0, 0, 1]
284  addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
285  break;
286  default:
287  case WorldFrame::ENU:
288  // Gravity: [0, 0, 1]
289  addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
290  break;
291  }
292 
293  normalizeQuaternion(s0, s1, s2, s3);
294 
295  // Apply feedback step
296  qDot1 -= gain_ * s0;
297  qDot2 -= gain_ * s1;
298  qDot3 -= gain_ * s2;
299  qDot4 -= gain_ * s3;
300  }
301 
302  // Integrate rate of change of quaternion to yield quaternion
303  q0 += qDot1 * dt;
304  q1 += qDot2 * dt;
305  q2 += qDot3 * dt;
306  q3 += qDot4 * dt;
307 
308  // Normalise quaternion
310 }
311 
312 
313 void ImuFilter::getGravity(float& rx, float& ry, float& rz,
314  float gravity)
315 {
316  // Estimate gravity vector from current orientation
317  switch (world_frame_) {
318  case WorldFrame::NED:
319  // Gravity: [0, 0, -1]
321  0.0, 0.0, -2.0*gravity,
322  rx, ry, rz);
323  break;
324  case WorldFrame::NWU:
325  // Gravity: [0, 0, 1]
327  0.0, 0.0, 2.0*gravity,
328  rx, ry, rz);
329  break;
330  default:
331  case WorldFrame::ENU:
332  // Gravity: [0, 0, 1]
334  0.0, 0.0, 2.0*gravity,
335  rx, ry, rz);
336  break;
337  }
338 }
static void compensateGyroDrift(float q0, float q1, float q2, float q3, float s0, float s1, float s2, float s3, float dt, float zeta, float &w_bx, float &w_by, float &w_bz, float &gx, float &gy, float &gz)
Definition: imu_filter.cpp:82
static float invSqrt(float x)
Definition: imu_filter.cpp:30
double q3
Definition: imu_filter.h:46
f
double q2
Definition: imu_filter.h:46
void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
Definition: imu_filter.cpp:175
void getGravity(float &rx, float &ry, float &rz, float gravity=9.80665)
Definition: imu_filter.cpp:313
static void normalizeVector(T &vx, T &vy, T &vz)
Definition: imu_filter.cpp:46
static void rotateAndScaleVector(float q0, float q1, float q2, float q3, float _2dx, float _2dy, float _2dz, float &rx, float &ry, float &rz)
Definition: imu_filter.cpp:64
double q0
Definition: imu_filter.h:46
void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt)
Definition: imu_filter.cpp:258
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: imu_filter.cpp:103
virtual ~ImuFilter()
Definition: imu_filter.cpp:171
float w_bx_
Definition: imu_filter.h:47
static void compensateMagneticDistortion(float q0, float q1, float q2, float q3, float mx, float my, float mz, float &_2bxy, float &_2bz)
Definition: imu_filter.cpp:149
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
float w_by_
Definition: imu_filter.h:47
double gain_
Definition: imu_filter.h:41
static void normalizeQuaternion(T &q0, T &q1, T &q2, T &q3)
Definition: imu_filter.cpp:55
double q1
Definition: imu_filter.h:46
float w_bz_
Definition: imu_filter.h:47
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: imu_filter.cpp:116
double zeta_
Definition: imu_filter.h:42
WorldFrame::WorldFrame world_frame_
Definition: imu_filter.h:43


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Thu Jun 18 2020 03:40:00