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 ImuFilter::ImuFilter():
00029 q0(1.0), q1(0.0), q2(0.0), q3(0.0),
00030 w_bx_(0.0), w_by_(0.0), w_bz_(0.0),
00031 zeta_(0.0), gain_(0.0)
00032 {
00033 }
00034
00035 ImuFilter::~ImuFilter()
00036 {
00037 }
00038
00039 void ImuFilter::madgwickAHRSupdate(
00040 float gx, float gy, float gz,
00041 float ax, float ay, float az,
00042 float mx, float my, float mz,
00043 float dt)
00044 {
00045 float recipNorm;
00046 float s0, s1, s2, s3;
00047 float qDot1, qDot2, qDot3, qDot4;
00048 float hx, hy;
00049 float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
00050 float _w_err_x, _w_err_y, _w_err_z;
00051
00052
00053 if(!std::isfinite(mx) || !std::isfinite(my) || !std::isfinite(mz))
00054 {
00055 madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);
00056 return;
00057 }
00058
00059
00060 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
00061 {
00062
00063 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00064 ax *= recipNorm;
00065 ay *= recipNorm;
00066 az *= recipNorm;
00067
00068
00069 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
00070 mx *= recipNorm;
00071 my *= recipNorm;
00072 mz *= recipNorm;
00073
00074
00075 _2q0mx = 2.0f * q0 * mx;
00076 _2q0my = 2.0f * q0 * my;
00077 _2q0mz = 2.0f * q0 * mz;
00078 _2q1mx = 2.0f * q1 * mx;
00079 _2q0 = 2.0f * q0;
00080 _2q1 = 2.0f * q1;
00081 _2q2 = 2.0f * q2;
00082 _2q3 = 2.0f * q3;
00083 _2q0q2 = 2.0f * q0 * q2;
00084 _2q2q3 = 2.0f * q2 * q3;
00085 q0q0 = q0 * q0;
00086 q0q1 = q0 * q1;
00087 q0q2 = q0 * q2;
00088 q0q3 = q0 * q3;
00089 q1q1 = q1 * q1;
00090 q1q2 = q1 * q2;
00091 q1q3 = q1 * q3;
00092 q2q2 = q2 * q2;
00093 q2q3 = q2 * q3;
00094 q3q3 = q3 * q3;
00095
00096
00097 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
00098 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
00099 _2bx = sqrt(hx * hx + hy * hy);
00100 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
00101 _4bx = 2.0f * _2bx;
00102 _4bz = 2.0f * _2bz;
00103
00104
00105 s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
00106 s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
00107 s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
00108 s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
00109 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
00110 s0 *= recipNorm;
00111 s1 *= recipNorm;
00112 s2 *= recipNorm;
00113 s3 *= recipNorm;
00114
00115
00116 _w_err_x = _2q0 * s1 - _2q1 * s0 - _2q2 * s3 + _2q3 * s2;
00117 _w_err_y = _2q0 * s2 + _2q1 * s3 - _2q2 * s0 - _2q3 * s1;
00118 _w_err_z = _2q0 * s3 - _2q1 * s2 + _2q2 * s1 - _2q3 * s0;
00119
00120 w_bx_ += _w_err_x * dt * zeta_;
00121 w_by_ += _w_err_y * dt * zeta_;
00122 w_bz_ += _w_err_z * dt * zeta_;
00123
00124 gx -= w_bx_;
00125 gy -= w_by_;
00126 gz -= w_bz_;
00127
00128
00129 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00130 qDot2 = 0.5f * ( q0 * gx + q2 * gz - q3 * gy);
00131 qDot3 = 0.5f * ( q0 * gy - q1 * gz + q3 * gx);
00132 qDot4 = 0.5f * ( q0 * gz + q1 * gy - q2 * gx);
00133
00134
00135 qDot1 -= gain_ * s0;
00136 qDot2 -= gain_ * s1;
00137 qDot3 -= gain_ * s2;
00138 qDot4 -= gain_ * s3;
00139 } else{
00140
00141 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00142 qDot2 = 0.5f * ( q0 * gx + q2 * gz - q3 * gy);
00143 qDot3 = 0.5f * ( q0 * gy - q1 * gz + q3 * gx);
00144 qDot4 = 0.5f * ( q0 * gz + q1 * gy - q2 * gx);
00145 }
00146
00147
00148 q0 += qDot1 * dt;
00149 q1 += qDot2 * dt;
00150 q2 += qDot3 * dt;
00151 q3 += qDot4 * dt;
00152
00153
00154 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00155 q0 *= recipNorm;
00156 q1 *= recipNorm;
00157 q2 *= recipNorm;
00158 q3 *= recipNorm;
00159 }
00160
00161 void ImuFilter::madgwickAHRSupdateIMU(
00162 float gx, float gy, float gz,
00163 float ax, float ay, float az,
00164 float dt)
00165 {
00166 float recipNorm;
00167 float s0, s1, s2, s3;
00168 float qDot1, qDot2, qDot3, qDot4;
00169 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
00170
00171
00172 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00173 qDot2 = 0.5f * ( q0 * gx + q2 * gz - q3 * gy);
00174 qDot3 = 0.5f * ( q0 * gy - q1 * gz + q3 * gx);
00175 qDot4 = 0.5f * ( q0 * gz + q1 * gy - q2 * gx);
00176
00177
00178 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
00179 {
00180
00181 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00182 ax *= recipNorm;
00183 ay *= recipNorm;
00184 az *= recipNorm;
00185
00186
00187 _2q0 = 2.0f * q0;
00188 _2q1 = 2.0f * q1;
00189 _2q2 = 2.0f * q2;
00190 _2q3 = 2.0f * q3;
00191 _4q0 = 4.0f * q0;
00192 _4q1 = 4.0f * q1;
00193 _4q2 = 4.0f * q2;
00194 _8q1 = 8.0f * q1;
00195 _8q2 = 8.0f * q2;
00196 q0q0 = q0 * q0;
00197 q1q1 = q1 * q1;
00198 q2q2 = q2 * q2;
00199 q3q3 = q3 * q3;
00200
00201
00202 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
00203 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
00204 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
00205 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
00206 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
00207 s0 *= recipNorm;
00208 s1 *= recipNorm;
00209 s2 *= recipNorm;
00210 s3 *= recipNorm;
00211
00212
00213 qDot1 -= gain_ * s0;
00214 qDot2 -= gain_ * s1;
00215 qDot3 -= gain_ * s2;
00216 qDot4 -= gain_ * s3;
00217 }
00218
00219
00220 q0 += qDot1 * dt;
00221 q1 += qDot2 * dt;
00222 q2 += qDot3 * dt;
00223 q3 += qDot4 * dt;
00224
00225
00226 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00227 q0 *= recipNorm;
00228 q1 *= recipNorm;
00229 q2 *= recipNorm;
00230 q3 *= recipNorm;
00231 }