MadgwickAHRS.cpp

00001 //=============================================================================================
00002 // MadgwickAHRS.c
00003 //=============================================================================================
00004 //
00005 // Implementation of Madgwick's IMU and AHRS algorithms.
00006 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
00007 //
00008 // From the x-io website "Open-source resources available on this website are
00009 // provided under the GNU General Public Licence unless an alternative licence
00010 // is provided in source."
00011 //
00012 // Date         Author          Notes
00013 // 29/09/2011   SOH Madgwick    Initial release
00014 // 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
00015 // 19/02/2012   SOH Madgwick    Magnetometer measurement is normalised
00016 //
00017 //=============================================================================================
00018 
00019 //-------------------------------------------------------------------------------------------
00020 // Header files
00021 
00022 #include "MadgwickAHRS.h"
00023 #include <math.h>
00024 
00025 //-------------------------------------------------------------------------------------------
00026 // Definitions
00027 
00028 #define sampleFreqDef   1500.0f          // sample frequency in Hz
00029 #define betaDef         0.1f            // 2 * proportional gain 0.1 - 0.5 - 5 was 2.5
00030 
00031 
00032 //============================================================================================
00033 // Functions
00034 
00035 //-------------------------------------------------------------------------------------------
00036 // AHRS algorithm update
00037 
00038 Madgwick::Madgwick()
00039 {
00040     beta = betaDef;
00041     q0 = 1.0f;
00042     q1 = 0.0f;
00043     q2 = 0.0f;
00044     q3 = 0.0f;
00045     invSampleFreq = 1.0f / sampleFreqDef;
00046     anglesComputed = 0;
00047 }
00048 
00049 void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
00050 {
00051     float recipNorm;
00052     float s0, s1, s2, s3;
00053     float qDot1, qDot2, qDot3, qDot4;
00054     float hx, hy;
00055     float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _8bx, _8bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
00056 
00057     // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
00058     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
00059         updateIMU(gx, gy, gz, ax, ay, az);
00060         return;
00061     }
00062 
00063     // Convert gyroscope degrees/sec to radians/sec
00064     gx *= 0.0174533f;
00065     gy *= 0.0174533f;
00066     gz *= 0.0174533f;
00067 
00068     // Rate of change of quaternion from gyroscope
00069     qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00070     qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
00071     qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
00072     qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
00073 
00074     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00075     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
00076 
00077         // Normalise accelerometer measurement
00078         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00079         ax *= recipNorm;
00080         ay *= recipNorm;
00081         az *= recipNorm;
00082 
00083         // Normalise magnetometer measurement
00084         recipNorm = invSqrt(mx * mx + my * my + mz * mz);
00085         mx *= recipNorm;
00086         my *= recipNorm;
00087         mz *= recipNorm;
00088 
00089         // Auxiliary variables to avoid repeated arithmetic
00090         _2q0mx = 2.0f * q0 * mx;
00091         _2q0my = 2.0f * q0 * my;
00092         _2q0mz = 2.0f * q0 * mz;
00093         _2q1mx = 2.0f * q1 * mx;
00094         _2q0 = 2.0f * q0;
00095         _2q1 = 2.0f * q1;
00096         _2q2 = 2.0f * q2;
00097         _2q3 = 2.0f * q3;
00098         _2q0q2 = 2.0f * q0 * q2;
00099         _2q2q3 = 2.0f * q2 * q3;
00100         q0q0 = q0 * q0;
00101         q0q1 = q0 * q1;
00102         q0q2 = q0 * q2;
00103         q0q3 = q0 * q3;
00104         q1q1 = q1 * q1;
00105         q1q2 = q1 * q2;
00106         q1q3 = q1 * q3;
00107         q2q2 = q2 * q2;
00108         q2q3 = q2 * q3;
00109         q3q3 = q3 * q3;
00110         /*
00111                 // Reference direction of Earth's magnetic field
00112                 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
00113                 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
00114                 _2bx = sqrtf(hx * hx + hy * hy);
00115                 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
00116                 _4bx = 2.0f * _2bx;
00117                 _4bz = 2.0f * _2bz;
00118 
00119                 // Gradient decent algorithm corrective step
00120                 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);
00121                 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);
00122                 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);
00123                 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);
00124                 */
00125         // Reference direction of Earth's magnetic field
00126         hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
00127         hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
00128         _2bx = sqrt(hx * hx + hy * hy);
00129         _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
00130         _4bx = 2.0f * _2bx;
00131         _4bz = 2.0f * _2bz;
00132         _8bx = 2.0f * _4bx;
00133         _8bz = 2.0f * _4bz;
00134 
00135         // Gradient decent algorithm corrective step
00136         s0= -_2q2*(2.0f*(q1q3 - q0q2) - ax)    +   _2q1*(2.0f*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
00137         s1= _2q3*(2.0f*(q1q3 - q0q2) - ax) +   _2q0*(2.0f*(q0q1 + q2q3) - ay) +   -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
00138         s2= -_2q0*(2.0f*(q1q3 - q0q2) - ax)    +     _2q3*(2.0f*(q0q1 + q2q3) - ay)   +   (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
00139         s3= _2q1*(2.0f*(q1q3 - q0q2) - ax) +   _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
00140         recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
00141         s0 *= recipNorm;
00142         s1 *= recipNorm;
00143         s2 *= recipNorm;
00144         s3 *= recipNorm;
00145 
00146         // Apply feedback step
00147         qDot1 -= beta * s0;
00148         qDot2 -= beta * s1;
00149         qDot3 -= beta * s2;
00150         qDot4 -= beta * s3;
00151     }
00152 
00153     // Integrate rate of change of quaternion to yield quaternion
00154     q0 += qDot1 * invSampleFreq;
00155     q1 += qDot2 * invSampleFreq;
00156     q2 += qDot3 * invSampleFreq;
00157     q3 += qDot4 * invSampleFreq;
00158 
00159     // Normalise quaternion
00160     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00161     q0 *= recipNorm;
00162     q1 *= recipNorm;
00163     q2 *= recipNorm;
00164     q3 *= recipNorm;
00165     anglesComputed = 0;
00166 }
00167 
00168 //-------------------------------------------------------------------------------------------
00169 // IMU algorithm update
00170 
00171 void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
00172 {
00173     float recipNorm;
00174     float s0, s1, s2, s3;
00175     float qDot1, qDot2, qDot3, qDot4;
00176     float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
00177 
00178     // Convert gyroscope degrees/sec to radians/sec
00179     gx *= 0.0174533f;
00180     gy *= 0.0174533f;
00181     gz *= 0.0174533f;
00182 
00183     // Rate of change of quaternion from gyroscope
00184     qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00185     qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
00186     qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
00187     qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
00188 
00189     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00190     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
00191 
00192         // Normalise accelerometer measurement
00193         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00194         ax *= recipNorm;
00195         ay *= recipNorm;
00196         az *= recipNorm;
00197 
00198         // Auxiliary variables to avoid repeated arithmetic
00199         _2q0 = 2.0f * q0;
00200         _2q1 = 2.0f * q1;
00201         _2q2 = 2.0f * q2;
00202         _2q3 = 2.0f * q3;
00203         _4q0 = 4.0f * q0;
00204         _4q1 = 4.0f * q1;
00205         _4q2 = 4.0f * q2;
00206         _8q1 = 8.0f * q1;
00207         _8q2 = 8.0f * q2;
00208         q0q0 = q0 * q0;
00209         q1q1 = q1 * q1;
00210         q2q2 = q2 * q2;
00211         q3q3 = q3 * q3;
00212 
00213         // Gradient decent algorithm corrective step
00214         s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
00215         s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
00216         s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
00217         s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
00218         recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
00219         s0 *= recipNorm;
00220         s1 *= recipNorm;
00221         s2 *= recipNorm;
00222         s3 *= recipNorm;
00223 
00224         // Apply feedback step
00225         qDot1 -= beta * s0;
00226         qDot2 -= beta * s1;
00227         qDot3 -= beta * s2;
00228         qDot4 -= beta * s3;
00229     }
00230 
00231     // Integrate rate of change of quaternion to yield quaternion
00232     q0 += qDot1 * invSampleFreq;
00233     q1 += qDot2 * invSampleFreq;
00234     q2 += qDot3 * invSampleFreq;
00235     q3 += qDot4 * invSampleFreq;
00236 
00237     // Normalise quaternion
00238     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00239     q0 *= recipNorm;
00240     q1 *= recipNorm;
00241     q2 *= recipNorm;
00242     q3 *= recipNorm;
00243     anglesComputed = 0;
00244 }
00245 
00246 //-------------------------------------------------------------------------------------------
00247 // Fast inverse square-root
00248 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
00249 
00250 /*float Madgwick::invSqrt(float x) {
00251     float halfx = 0.5f * x;
00252     float y = x;
00253     long i = *(long*)&y;
00254     i = 0x5f3759df - (i>>1);
00255     y = *(float*)&i;
00256     y = y * (1.5f - (halfx * y * y));
00257     y = y * (1.5f - (halfx * y * y));
00258     return y;
00259 }
00260 
00261 float Madgwick::invSqrt(float x){
00262    unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
00263    float tmp = *(float*)&i;
00264    return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
00265 }
00266 */
00267 float Madgwick::invSqrt(float x)
00268 {
00269     float tmp = 1/(sqrt(x));
00270     return tmp;
00271 }
00272 
00273 //-------------------------------------------------------------------------------------------
00274 
00275 void Madgwick::computeAngles()
00276 {
00277     roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
00278     pitch = asinf(-2.0f * (q1*q3 - q0*q2));
00279     yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
00280     anglesComputed = 1;
00281 }
00282