MahonyAHRS.cpp

00001 //=============================================================================================
00002 // MahonyAHRS.c
00003 //=============================================================================================
00004 //
00005 // Madgwick's implementation of Mayhony's AHRS algorithm.
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 //
00016 // Algorithm paper:
00017 // http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
00018 //
00019 //=============================================================================================
00020 
00021 //-------------------------------------------------------------------------------------------
00022 // Header files
00023 
00024 #include "MahonyAHRS.h"
00025 #include <math.h>
00026 
00027 //-------------------------------------------------------------------------------------------
00028 // Definitions
00029 
00030 #define DEFAULT_SAMPLE_FREQ 1500.0f  // sample frequency in Hz
00031 #define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
00032 #define twoKiDef    (2.0f * 0.1f)   // 2 * integral gain
00033 
00034 
00035 //============================================================================================
00036 // Functions
00037 
00038 //-------------------------------------------------------------------------------------------
00039 // AHRS algorithm update
00040 
00041 Mahony::Mahony()
00042 {
00043     twoKp = twoKpDef;   // 2 * proportional gain (Kp)
00044     twoKi = twoKiDef;   // 2 * integral gain (Ki)
00045     q0 = 1.0f;
00046     q1 = 0.0f;
00047     q2 = 0.0f;
00048     q3 = 0.0f;
00049     integralFBx = 0.0f;
00050     integralFBy = 0.0f;
00051     integralFBz = 0.0f;
00052     anglesComputed = 0;
00053     invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
00054 }
00055 
00056 void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
00057 {
00058     float recipNorm;
00059     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
00060     float hx, hy, bx, bz;
00061     float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
00062     float halfex, halfey, halfez;
00063     float qa, qb, qc;
00064 
00065     // Use IMU algorithm if magnetometer measurement invalid
00066     // (avoids NaN in magnetometer normalisation)
00067     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
00068         updateIMU(gx, gy, gz, ax, ay, az);
00069         return;
00070     }
00071 
00072     // Convert gyroscope degrees/sec to radians/sec
00073     gx *= 0.0174533f;
00074     gy *= 0.0174533f;
00075     gz *= 0.0174533f;
00076 
00077     // Compute feedback only if accelerometer measurement valid
00078     // (avoids NaN in accelerometer normalisation)
00079     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
00080 
00081         // Normalise accelerometer measurement
00082         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00083         ax *= recipNorm;
00084         ay *= recipNorm;
00085         az *= recipNorm;
00086 
00087         // Normalise magnetometer measurement
00088         recipNorm = invSqrt(mx * mx + my * my + mz * mz);
00089         mx *= recipNorm;
00090         my *= recipNorm;
00091         mz *= recipNorm;
00092 
00093         // Auxiliary variables to avoid repeated arithmetic
00094         q0q0 = q0 * q0;
00095         q0q1 = q0 * q1;
00096         q0q2 = q0 * q2;
00097         q0q3 = q0 * q3;
00098         q1q1 = q1 * q1;
00099         q1q2 = q1 * q2;
00100         q1q3 = q1 * q3;
00101         q2q2 = q2 * q2;
00102         q2q3 = q2 * q3;
00103         q3q3 = q3 * q3;
00104 
00105         // Reference direction of Earth's magnetic field
00106         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
00107         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
00108         bx = sqrtf(hx * hx + hy * hy);
00109         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
00110 
00111         // Estimated direction of gravity and magnetic field
00112         halfvx = q1q3 - q0q2;
00113         halfvy = q0q1 + q2q3;
00114         halfvz = q0q0 - 0.5f + q3q3;
00115         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
00116         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
00117         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
00118 
00119         // Error is sum of cross product between estimated direction
00120         // and measured direction of field vectors
00121         halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
00122         halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
00123         halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
00124 
00125         // Compute and apply integral feedback if enabled
00126         if(twoKi > 0.0f) {
00127             // integral error scaled by Ki
00128             integralFBx += twoKi * halfex * invSampleFreq;
00129             integralFBy += twoKi * halfey * invSampleFreq;
00130             integralFBz += twoKi * halfez * invSampleFreq;
00131             gx += integralFBx;  // apply integral feedback
00132             gy += integralFBy;
00133             gz += integralFBz;
00134         } else {
00135             integralFBx = 0.0f; // prevent integral windup
00136             integralFBy = 0.0f;
00137             integralFBz = 0.0f;
00138         }
00139 
00140         // Apply proportional feedback
00141         gx += twoKp * halfex;
00142         gy += twoKp * halfey;
00143         gz += twoKp * halfez;
00144     }
00145 
00146     // Integrate rate of change of quaternion
00147     gx *= (0.5f * invSampleFreq);       // pre-multiply common factors
00148     gy *= (0.5f * invSampleFreq);
00149     gz *= (0.5f * invSampleFreq);
00150     qa = q0;
00151     qb = q1;
00152     qc = q2;
00153     q0 += (-qb * gx - qc * gy - q3 * gz);
00154     q1 += (qa * gx + qc * gz - q3 * gy);
00155     q2 += (qa * gy - qb * gz + q3 * gx);
00156     q3 += (qa * gz + qb * gy - qc * gx);
00157 
00158     // Normalise quaternion
00159     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00160     q0 *= recipNorm;
00161     q1 *= recipNorm;
00162     q2 *= recipNorm;
00163     q3 *= recipNorm;
00164     anglesComputed = 0;
00165 }
00166 
00167 //-------------------------------------------------------------------------------------------
00168 // IMU algorithm update
00169 
00170 void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
00171 {
00172     float recipNorm;
00173     float halfvx, halfvy, halfvz;
00174     float halfex, halfey, halfez;
00175     float qa, qb, qc;
00176 
00177     // Convert gyroscope degrees/sec to radians/sec
00178     gx *= 0.0174533f;
00179     gy *= 0.0174533f;
00180     gz *= 0.0174533f;
00181 
00182     // Compute feedback only if accelerometer measurement valid
00183     // (avoids NaN in accelerometer normalisation)
00184     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
00185 
00186         // Normalise accelerometer measurement
00187         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00188         ax *= recipNorm;
00189         ay *= recipNorm;
00190         az *= recipNorm;
00191 
00192         // Estimated direction of gravity
00193         halfvx = q1 * q3 - q0 * q2;
00194         halfvy = q0 * q1 + q2 * q3;
00195         halfvz = q0 * q0 - 0.5f + q3 * q3;
00196 
00197         // Error is sum of cross product between estimated
00198         // and measured direction of gravity
00199         halfex = (ay * halfvz - az * halfvy);
00200         halfey = (az * halfvx - ax * halfvz);
00201         halfez = (ax * halfvy - ay * halfvx);
00202 
00203         // Compute and apply integral feedback if enabled
00204         if(twoKi > 0.0f) {
00205             // integral error scaled by Ki
00206             integralFBx += twoKi * halfex * invSampleFreq;
00207             integralFBy += twoKi * halfey * invSampleFreq;
00208             integralFBz += twoKi * halfez * invSampleFreq;
00209             gx += integralFBx;  // apply integral feedback
00210             gy += integralFBy;
00211             gz += integralFBz;
00212         } else {
00213             integralFBx = 0.0f; // prevent integral windup
00214             integralFBy = 0.0f;
00215             integralFBz = 0.0f;
00216         }
00217 
00218         // Apply proportional feedback
00219         gx += twoKp * halfex;
00220         gy += twoKp * halfey;
00221         gz += twoKp * halfez;
00222     }
00223 
00224     // Integrate rate of change of quaternion
00225     gx *= (0.5f * invSampleFreq);       // pre-multiply common factors
00226     gy *= (0.5f * invSampleFreq);
00227     gz *= (0.5f * invSampleFreq);
00228     qa = q0;
00229     qb = q1;
00230     qc = q2;
00231     q0 += (-qb * gx - qc * gy - q3 * gz);
00232     q1 += (qa * gx + qc * gz - q3 * gy);
00233     q2 += (qa * gy - qb * gz + q3 * gx);
00234     q3 += (qa * gz + qb * gy - qc * gx);
00235 
00236     // Normalise quaternion
00237     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00238     q0 *= recipNorm;
00239     q1 *= recipNorm;
00240     q2 *= recipNorm;
00241     q3 *= recipNorm;
00242     anglesComputed = 0;
00243 }
00244 
00245 //-------------------------------------------------------------------------------------------
00246 // Fast inverse square-root
00247 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
00248 /*
00249 float Mahony::invSqrt(float x)
00250 {
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 /*
00262 float Mahony::invSqrt(float x){
00263    unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
00264    float tmp = *(float*)&i;
00265    return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
00266 }
00267 */
00268 
00269 float Mahony::invSqrt(float x)
00270 {
00271     float temp = 1/(sqrt(x));
00272     return temp;
00273 }
00274 
00275 //-------------------------------------------------------------------------------------------
00276 
00277 void Mahony::computeAngles()
00278 {
00279     roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
00280     pitch = asinf(-2.0f * (q1*q3 - q0*q2));
00281     yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
00282     anglesComputed = 1;
00283 }
00284 
00285 
00286 //============================================================================================
00287 // END OF CODE
00288 //============================================================================================