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 #include <cmath>
00026 #include "imu_filter_madgwick/imu_filter.h"
00027
00028
00029
00030 static float invSqrt(float x)
00031 {
00032 float xhalf = 0.5f * x;
00033 union
00034 {
00035 float x;
00036 int i;
00037 } u;
00038 u.x = x;
00039 u.i = 0x5f3759df - (u.i >> 1);
00040
00041 u.x = u.x * (1.5f - xhalf * u.x * u.x);
00042 return u.x;
00043 }
00044
00045 template<typename T>
00046 static inline void normalizeVector(T& vx, T& vy, T& vz)
00047 {
00048 T recipNorm = invSqrt (vx * vx + vy * vy + vz * vz);
00049 vx *= recipNorm;
00050 vy *= recipNorm;
00051 vz *= recipNorm;
00052 }
00053
00054 template<typename T>
00055 static inline void normalizeQuaternion(T& q0, T& q1, T& q2, T& q3)
00056 {
00057 T recipNorm = invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00058 q0 *= recipNorm;
00059 q1 *= recipNorm;
00060 q2 *= recipNorm;
00061 q3 *= recipNorm;
00062 }
00063
00064 static inline void rotateAndScaleVector(
00065 float q0, float q1, float q2, float q3,
00066 float _2dx, float _2dy, float _2dz,
00067 float& rx, float& ry, float& rz) {
00068
00069
00070 rx = _2dx * (0.5f - q2 * q2 - q3 * q3)
00071 + _2dy * (q0 * q3 + q1 * q2)
00072 + _2dz * (q1 * q3 - q0 * q2);
00073 ry = _2dx * (q1 * q2 - q0 * q3)
00074 + _2dy * (0.5f - q1 * q1 - q3 * q3)
00075 + _2dz * (q0 * q1 + q2 * q3);
00076 rz = _2dx * (q0 * q2 + q1 * q3)
00077 + _2dy * (q2 * q3 - q0 * q1)
00078 + _2dz * (0.5f - q1 * q1 - q2 * q2);
00079 }
00080
00081
00082 static inline void compensateGyroDrift(
00083 float q0, float q1, float q2, float q3,
00084 float s0, float s1, float s2, float s3,
00085 float dt, float zeta,
00086 float& w_bx, float& w_by, float& w_bz,
00087 float& gx, float& gy, float& gz)
00088 {
00089
00090 float w_err_x = 2.0f * q0 * s1 - 2.0f * q1 * s0 - 2.0f * q2 * s3 + 2.0f * q3 * s2;
00091 float w_err_y = 2.0f * q0 * s2 + 2.0f * q1 * s3 - 2.0f * q2 * s0 - 2.0f * q3 * s1;
00092 float w_err_z = 2.0f * q0 * s3 - 2.0f * q1 * s2 + 2.0f * q2 * s1 - 2.0f * q3 * s0;
00093
00094 w_bx += w_err_x * dt * zeta;
00095 w_by += w_err_y * dt * zeta;
00096 w_bz += w_err_z * dt * zeta;
00097
00098 gx -= w_bx;
00099 gy -= w_by;
00100 gz -= w_bz;
00101 }
00102
00103 static inline void orientationChangeFromGyro(
00104 float q0, float q1, float q2, float q3,
00105 float gx, float gy, float gz,
00106 float& qDot1, float& qDot2, float& qDot3, float& qDot4)
00107 {
00108
00109
00110 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00111 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
00112 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
00113 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
00114 }
00115
00116 static inline void addGradientDescentStep(
00117 float q0, float q1, float q2, float q3,
00118 float _2dx, float _2dy, float _2dz,
00119 float mx, float my, float mz,
00120 float& s0, float& s1, float& s2, float& s3)
00121 {
00122 float f0, f1, f2;
00123
00124
00125
00126 rotateAndScaleVector(q0,q1,q2,q3, _2dx, _2dy, _2dz, f0, f1, f2);
00127
00128 f0 -= mx;
00129 f1 -= my;
00130 f2 -= mz;
00131
00132
00133
00134
00135 s0 += (_2dy * q3 - _2dz * q2) * f0
00136 + (-_2dx * q3 + _2dz * q1) * f1
00137 + (_2dx * q2 - _2dy * q1) * f2;
00138 s1 += (_2dy * q2 + _2dz * q3) * f0
00139 + (_2dx * q2 - 2.0f * _2dy * q1 + _2dz * q0) * f1
00140 + (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2;
00141 s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0
00142 + (_2dx * q1 + _2dz * q3) * f1
00143 + (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2;
00144 s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0
00145 + (-_2dx * q0 - 2.0f * _2dy * q3 + _2dz * q2) * f1
00146 + (_2dx * q1 + _2dy * q2) * f2;
00147 }
00148
00149 static inline void compensateMagneticDistortion(
00150 float q0, float q1, float q2, float q3,
00151 float mx, float my, float mz,
00152 float& _2bxy, float& _2bz)
00153 {
00154 float hx, hy, hz;
00155
00156 rotateAndScaleVector(q0, -q1, -q2, -q3, mx, my, mz, hx, hy, hz);
00157
00158 _2bxy = 4.0f * sqrt (hx * hx + hy * hy);
00159 _2bz = 4.0f * hz;
00160
00161 }
00162
00163
00164 ImuFilter::ImuFilter() :
00165 q0(1.0), q1(0.0), q2(0.0), q3(0.0),
00166 w_bx_(0.0), w_by_(0.0), w_bz_(0.0),
00167 zeta_ (0.0), gain_ (0.0), world_frame_(WorldFrame::ENU)
00168 {
00169 }
00170
00171 ImuFilter::~ImuFilter()
00172 {
00173 }
00174
00175 void ImuFilter::madgwickAHRSupdate(
00176 float gx, float gy, float gz,
00177 float ax, float ay, float az,
00178 float mx, float my, float mz,
00179 float dt)
00180 {
00181 float s0, s1, s2, s3;
00182 float qDot1, qDot2, qDot3, qDot4;
00183 float _2bz, _2bxy;
00184
00185
00186 if (!std::isfinite(mx) || !std::isfinite(my) || !std::isfinite(mz))
00187 {
00188 madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);
00189 return;
00190 }
00191
00192
00193 if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
00194 {
00195
00196 normalizeVector(ax, ay, az);
00197
00198
00199 normalizeVector(mx, my, mz);
00200
00201
00202 compensateMagneticDistortion(q0, q1, q2, q3, mx, my, mz, _2bxy, _2bz);
00203
00204
00205 s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
00206 switch (world_frame_) {
00207 case WorldFrame::NED:
00208
00209 addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
00210
00211
00212 addGradientDescentStep(q0,q1,q2,q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
00213 break;
00214 case WorldFrame::NWU:
00215
00216 addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
00217
00218
00219 addGradientDescentStep(q0,q1,q2,q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
00220 break;
00221 default:
00222 case WorldFrame::ENU:
00223
00224 addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
00225
00226
00227 addGradientDescentStep(q0, q1, q2, q3, 0.0, _2bxy, _2bz, mx, my, mz, s0, s1, s2, s3);
00228 break;
00229 }
00230 normalizeQuaternion(s0, s1, s2, s3);
00231
00232
00233 compensateGyroDrift(q0, q1, q2, q3, s0, s1, s2, s3, dt, zeta_, w_bx_, w_by_, w_bz_, gx, gy, gz);
00234
00235 orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
00236
00237
00238 qDot1 -= gain_ * s0;
00239 qDot2 -= gain_ * s1;
00240 qDot3 -= gain_ * s2;
00241 qDot4 -= gain_ * s3;
00242 }
00243 else
00244 {
00245 orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
00246 }
00247
00248
00249 q0 += qDot1 * dt;
00250 q1 += qDot2 * dt;
00251 q2 += qDot3 * dt;
00252 q3 += qDot4 * dt;
00253
00254
00255 normalizeQuaternion(q0, q1, q2, q3);
00256 }
00257
00258 void ImuFilter::madgwickAHRSupdateIMU(
00259 float gx, float gy, float gz,
00260 float ax, float ay, float az,
00261 float dt)
00262 {
00263 float recipNorm;
00264 float s0, s1, s2, s3;
00265 float qDot1, qDot2, qDot3, qDot4;
00266
00267
00268 orientationChangeFromGyro (q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
00269
00270
00271 if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
00272 {
00273
00274 normalizeVector(ax, ay, az);
00275
00276
00277 s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
00278 switch (world_frame_) {
00279 case WorldFrame::NED:
00280
00281 addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
00282 break;
00283 case WorldFrame::NWU:
00284
00285 addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
00286 break;
00287 default:
00288 case WorldFrame::ENU:
00289
00290 addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
00291 break;
00292 }
00293
00294 normalizeQuaternion(s0, s1, s2, s3);
00295
00296
00297 qDot1 -= gain_ * s0;
00298 qDot2 -= gain_ * s1;
00299 qDot3 -= gain_ * s2;
00300 qDot4 -= gain_ * s3;
00301 }
00302
00303
00304 q0 += qDot1 * dt;
00305 q1 += qDot2 * dt;
00306 q2 += qDot3 * dt;
00307 q3 += qDot4 * dt;
00308
00309
00310 normalizeQuaternion (q0, q1, q2, q3);
00311 }