ahrs.cpp

00001 // Implementation of Sebastian Madgwick's "...efficient orientation filter
00002 // for... inertial/magnetic sensor arrays"
00003 // (see http://www.x-io.co.uk/category/open-source/ for examples & more details)
00004 // which fuses acceleration, rotation rate, and magnetic moments to produce a
00005 // quaternion-based estimate of absolute device orientation -- which can be
00006 // converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
00007 // The performance of the orientation filter is at least as good as conventional
00008 // Kalman-based filtering algorithms but is much less computationally
00009 // intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
00010 
00011 #include "ahrs.h"
00012 #include <math.h>
00013 // These are the free parameters in the Mahony filter and fusion scheme, Kp
00014 // for proportional feedback, Ki for integral
00015 #define Kp 2.0f * 5.0f
00016 #define Ki 0.0f
00017 typedef unsigned char byte;
00018 static float GyroMeasError = PI * (40.0f / 180.0f);
00019 // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
00020 static float GyroMeasDrift = PI * (0.0f  / 180.0f);
00021 // There is a tradeoff in the beta parameter between accuracy and response
00022 // speed. In the original Madgwick study, beta of 0.041 (corresponding to
00023 // GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
00024 // However, with this value, the LSM9SD0 response time is about 10 seconds
00025 // to a stable initial quaternion. Subsequent changes also require a
00026 // longish lag time to a stable output, not fast enough for a quadcopter or
00027 // robot car! By increasing beta (GyroMeasError) by about a factor of
00028 // fifteen, the response time constant is reduced to ~2 sec. I haven't
00029 // noticed any reduction in solution accuracy. This is essentially the I
00030 // coefficient in a PID control sense; the bigger the feedback coefficient,
00031 // the faster the solution converges, usually at the expense of accuracy.
00032 // In any case, this is the free parameter in the Madgwick filtering and
00033 // fusion scheme.
00034 static float beta = sqrt(3.0f / 4.0f) * GyroMeasError;   // Compute beta
00035 // Compute zeta, the other free parameter in the Madgwick scheme usually
00036 // set to a small or zero value
00037 static float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
00038 
00039 // Vector to hold integral error for Mahony method
00040 static float eInt[3] = {0.0f, 0.0f, 0.0f};
00041 // Vector to hold quaternion
00042 static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
00043 
00044 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
00045 {
00046   // short name local variable for readability
00047   float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
00048   float norm;
00049   float hx, hy, _2bx, _2bz;
00050   float s1, s2, s3, s4;
00051   float qDot1, qDot2, qDot3, qDot4;
00052 
00053   // Auxiliary variables to avoid repeated arithmetic
00054   float _2q1mx;
00055   float _2q1my;
00056   float _2q1mz;
00057   float _2q2mx;
00058   float _4bx;
00059   float _4bz;
00060   float _2q1 = 2.0f * q1;
00061   float _2q2 = 2.0f * q2;
00062   float _2q3 = 2.0f * q3;
00063   float _2q4 = 2.0f * q4;
00064   float _2q1q3 = 2.0f * q1 * q3;
00065   float _2q3q4 = 2.0f * q3 * q4;
00066   float q1q1 = q1 * q1;
00067   float q1q2 = q1 * q2;
00068   float q1q3 = q1 * q3;
00069   float q1q4 = q1 * q4;
00070   float q2q2 = q2 * q2;
00071   float q2q3 = q2 * q3;
00072   float q2q4 = q2 * q4;
00073   float q3q3 = q3 * q3;
00074   float q3q4 = q3 * q4;
00075   float q4q4 = q4 * q4;
00076 
00077   // Normalise accelerometer measurement
00078   norm = sqrt(ax * ax + ay * ay + az * az);
00079   if (norm == 0.0f) return; // handle NaN
00080   norm = 1.0f/norm;
00081   ax *= norm;
00082   ay *= norm;
00083   az *= norm;
00084 
00085   // Normalise magnetometer measurement
00086   norm = sqrt(mx * mx + my * my + mz * mz);
00087   if (norm == 0.0f) return; // handle NaN
00088   norm = 1.0f/norm;
00089   mx *= norm;
00090   my *= norm;
00091   mz *= norm;
00092 
00093   // Reference direction of Earth's magnetic field
00094   _2q1mx = 2.0f * q1 * mx;
00095   _2q1my = 2.0f * q1 * my;
00096   _2q1mz = 2.0f * q1 * mz;
00097   _2q2mx = 2.0f * q2 * mx;
00098   hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 +
00099        _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
00100   hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
00101   _2bx = sqrt(hx * hx + hy * hy);
00102   _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
00103   _4bx = 2.0f * _2bx;
00104   _4bz = 2.0f * _2bz;
00105 
00106   // Gradient decent algorithm corrective step
00107   s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00108   s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00109   s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00110   s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00111   norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
00112   norm = 1.0f/norm;
00113   s1 *= norm;
00114   s2 *= norm;
00115   s3 *= norm;
00116   s4 *= norm;
00117 
00118   // Compute rate of change of quaternion
00119   qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
00120   qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
00121   qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
00122   qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
00123 
00124   // Integrate to yield quaternion
00125   q1 += qDot1 * deltat;
00126   q2 += qDot2 * deltat;
00127   q3 += qDot3 * deltat;
00128   q4 += qDot4 * deltat;
00129   norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
00130   norm = 1.0f/norm;
00131   q[0] = q1 * norm;
00132   q[1] = q2 * norm;
00133   q[2] = q3 * norm;
00134   q[3] = q4 * norm;
00135 }
00136 
00137 
00138 
00139 // Similar to Madgwick scheme but uses proportional and integral filtering on
00140 // the error between estimated reference vectors and measured ones.
00141 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
00142 {
00143   // short name local variable for readability
00144   float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
00145   float norm;
00146   float hx, hy, bx, bz;
00147   float vx, vy, vz, wx, wy, wz;
00148   float ex, ey, ez;
00149   float pa, pb, pc;
00150 
00151   // Auxiliary variables to avoid repeated arithmetic
00152   float q1q1 = q1 * q1;
00153   float q1q2 = q1 * q2;
00154   float q1q3 = q1 * q3;
00155   float q1q4 = q1 * q4;
00156   float q2q2 = q2 * q2;
00157   float q2q3 = q2 * q3;
00158   float q2q4 = q2 * q4;
00159   float q3q3 = q3 * q3;
00160   float q3q4 = q3 * q4;
00161   float q4q4 = q4 * q4;
00162 
00163   // Normalise accelerometer measurement
00164   norm = sqrt(ax * ax + ay * ay + az * az);
00165   if (norm == 0.0f) return; // Handle NaN
00166   norm = 1.0f / norm;       // Use reciprocal for division
00167   ax *= norm;
00168   ay *= norm;
00169   az *= norm;
00170 
00171   // Normalise magnetometer measurement
00172   norm = sqrt(mx * mx + my * my + mz * mz);
00173   if (norm == 0.0f) return; // Handle NaN
00174   norm = 1.0f / norm;       // Use reciprocal for division
00175   mx *= norm;
00176   my *= norm;
00177   mz *= norm;
00178 
00179   // Reference direction of Earth's magnetic field
00180   hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
00181   hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
00182   bx = sqrt((hx * hx) + (hy * hy));
00183   bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
00184 
00185   // Estimated direction of gravity and magnetic field
00186   vx = 2.0f * (q2q4 - q1q3);
00187   vy = 2.0f * (q1q2 + q3q4);
00188   vz = q1q1 - q2q2 - q3q3 + q4q4;
00189   wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
00190   wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
00191   wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
00192 
00193   // Error is cross product between estimated direction and measured direction of gravity
00194   ex = (ay * vz - az * vy) + (my * wz - mz * wy);
00195   ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
00196   ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
00197   if (Ki > 0.0f)
00198   {
00199     eInt[0] += ex;      // accumulate integral error
00200     eInt[1] += ey;
00201     eInt[2] += ez;
00202   }
00203   else
00204   {
00205     eInt[0] = 0.0f;     // prevent integral wind up
00206     eInt[1] = 0.0f;
00207     eInt[2] = 0.0f;
00208   }
00209 
00210   // Apply feedback terms
00211   gx = gx + Kp * ex + Ki * eInt[0];
00212   gy = gy + Kp * ey + Ki * eInt[1];
00213   gz = gz + Kp * ez + Ki * eInt[2];
00214  
00215   // Integrate rate of change of quaternion
00216   pa = q2;
00217   pb = q3;
00218   pc = q4;
00219   q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
00220   q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
00221   q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
00222   q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
00223 
00224   // Normalise quaternion
00225   norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
00226   norm = 1.0f / norm;
00227   q[0] = q1 * norm;
00228   q[1] = q2 * norm;
00229   q[2] = q3 * norm;
00230   q[3] = q4 * norm;
00231 }
00232 
00233 const float * getQ () { return q; }