AHRS Library
Embed:
(wiki syntax)
Show/hide line numbers
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 // 18/12/2016 Added better fast inverse square root 00017 // 00018 //============================================================================================= 00019 00020 //------------------------------------------------------------------------------------------- 00021 // Header files 00022 00023 #include "MadgwickAHRS.h" 00024 #include <math.h> 00025 00026 //------------------------------------------------------------------------------------------- 00027 // Definitions 00028 00029 #define betaDef 0.25f // 2 * proportional gain 0.1 - 0.5 - 5; 8 00030 00031 00032 //============================================================================================ 00033 // Functions 00034 00035 //------------------------------------------------------------------------------------------- 00036 // AHRS algorithm update 00037 00038 Madgwick::Madgwick(float Ts) { 00039 beta = betaDef; 00040 q0 = 1.0f; 00041 q1 = 0.0f; 00042 q2 = 0.0f; 00043 q3 = 0.0f; 00044 invSampleFreq = Ts; 00045 anglesComputed = 0; 00046 } 00047 00048 void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { 00049 float recipNorm; 00050 float s0, s1, s2, s3; 00051 float qDot1, qDot2, qDot3, qDot4; 00052 float hx, hy; 00053 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; 00054 00055 // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) 00056 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { 00057 updateIMU(gx, gy, gz, ax, ay, az); 00058 return; 00059 } 00060 00061 // Convert gyroscope degrees/sec to radians/sec 00062 gx *= 0.0174533f; 00063 gy *= 0.0174533f; 00064 gz *= 0.0174533f; 00065 00066 // Rate of change of quaternion from gyroscope 00067 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); 00068 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); 00069 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); 00070 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); 00071 00072 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 00073 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 00074 00075 // Normalise accelerometer measurement 00076 recipNorm = invSqrt(ax * ax + ay * ay + az * az); 00077 ax *= recipNorm; 00078 ay *= recipNorm; 00079 az *= recipNorm; 00080 00081 // Normalise magnetometer measurement 00082 recipNorm = invSqrt(mx * mx + my * my + mz * mz); 00083 mx *= recipNorm; 00084 my *= recipNorm; 00085 mz *= recipNorm; 00086 00087 // Auxiliary variables to avoid repeated arithmetic 00088 _2q0mx = 2.0f * q0 * mx; 00089 _2q0my = 2.0f * q0 * my; 00090 _2q0mz = 2.0f * q0 * mz; 00091 _2q1mx = 2.0f * q1 * mx; 00092 _2q0 = 2.0f * q0; 00093 _2q1 = 2.0f * q1; 00094 _2q2 = 2.0f * q2; 00095 _2q3 = 2.0f * q3; 00096 _2q0q2 = 2.0f * q0 * q2; 00097 _2q2q3 = 2.0f * q2 * q3; 00098 q0q0 = q0 * q0; 00099 q0q1 = q0 * q1; 00100 q0q2 = q0 * q2; 00101 q0q3 = q0 * q3; 00102 q1q1 = q1 * q1; 00103 q1q2 = q1 * q2; 00104 q1q3 = q1 * q3; 00105 q2q2 = q2 * q2; 00106 q2q3 = q2 * q3; 00107 q3q3 = q3 * q3; 00108 00109 // Reference direction of Earth's magnetic field 00110 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; 00111 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; 00112 _2bx = sqrtf(hx * hx + hy * hy); 00113 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; 00114 _4bx = 2.0f * _2bx; 00115 _4bz = 2.0f * _2bz; 00116 00117 // Gradient decent algorithm corrective step 00118 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); 00119 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); 00120 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); 00121 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); 00122 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 00123 s0 *= recipNorm; 00124 s1 *= recipNorm; 00125 s2 *= recipNorm; 00126 s3 *= recipNorm; 00127 00128 // Apply feedback step 00129 qDot1 -= beta * s0; 00130 qDot2 -= beta * s1; 00131 qDot3 -= beta * s2; 00132 qDot4 -= beta * s3; 00133 } 00134 00135 // Integrate rate of change of quaternion to yield quaternion 00136 q0 += qDot1 * invSampleFreq; 00137 q1 += qDot2 * invSampleFreq; 00138 q2 += qDot3 * invSampleFreq; 00139 q3 += qDot4 * invSampleFreq; 00140 00141 // Normalise quaternion 00142 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 00143 q0 *= recipNorm; 00144 q1 *= recipNorm; 00145 q2 *= recipNorm; 00146 q3 *= recipNorm; 00147 anglesComputed = 0; 00148 } 00149 00150 //------------------------------------------------------------------------------------------- 00151 // IMU algorithm update 00152 00153 void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { 00154 float recipNorm; 00155 float s0, s1, s2, s3; 00156 float qDot1, qDot2, qDot3, qDot4; 00157 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; 00158 00159 // Convert gyroscope degrees/sec to radians/sec 00160 gx *= 0.0174533f; 00161 gy *= 0.0174533f; 00162 gz *= 0.0174533f; 00163 00164 // Rate of change of quaternion from gyroscope 00165 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); 00166 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); 00167 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); 00168 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); 00169 00170 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 00171 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 00172 00173 // Normalise accelerometer measurement 00174 recipNorm = invSqrt(ax * ax + ay * ay + az * az); 00175 ax *= recipNorm; 00176 ay *= recipNorm; 00177 az *= recipNorm; 00178 00179 // Auxiliary variables to avoid repeated arithmetic 00180 _2q0 = 2.0f * q0; 00181 _2q1 = 2.0f * q1; 00182 _2q2 = 2.0f * q2; 00183 _2q3 = 2.0f * q3; 00184 _4q0 = 4.0f * q0; 00185 _4q1 = 4.0f * q1; 00186 _4q2 = 4.0f * q2; 00187 _8q1 = 8.0f * q1; 00188 _8q2 = 8.0f * q2; 00189 q0q0 = q0 * q0; 00190 q1q1 = q1 * q1; 00191 q2q2 = q2 * q2; 00192 q3q3 = q3 * q3; 00193 00194 // Gradient decent algorithm corrective step 00195 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; 00196 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; 00197 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; 00198 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; 00199 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 00200 s0 *= recipNorm; 00201 s1 *= recipNorm; 00202 s2 *= recipNorm; 00203 s3 *= recipNorm; 00204 00205 // Apply feedback step 00206 qDot1 -= beta * s0; 00207 qDot2 -= beta * s1; 00208 qDot3 -= beta * s2; 00209 qDot4 -= beta * s3; 00210 } 00211 00212 // Integrate rate of change of quaternion to yield quaternion 00213 q0 += qDot1 * invSampleFreq; 00214 q1 += qDot2 * invSampleFreq; 00215 q2 += qDot3 * invSampleFreq; 00216 q3 += qDot4 * invSampleFreq; 00217 00218 // Normalise quaternion 00219 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 00220 q0 *= recipNorm; 00221 q1 *= recipNorm; 00222 q2 *= recipNorm; 00223 q3 *= recipNorm; 00224 anglesComputed = 0; 00225 } 00226 00227 //------------------------------------------------------------------------------------------- 00228 // Fast inverse square-root 00229 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root 00230 00231 /*float Madgwick::invSqrt(float x) { 00232 float halfx = 0.5f * x; 00233 float y = x; 00234 long i = *(long*)&y; 00235 i = 0x5f3759df - (i>>1); 00236 y = *(float*)&i; 00237 y = y * (1.5f - (halfx * y * y)); 00238 y = y * (1.5f - (halfx * y * y)); 00239 return y; 00240 } */ 00241 00242 float Madgwick::invSqrt(float x){ 00243 unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1); 00244 float tmp = *(float*)&i; 00245 return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp); 00246 } 00247 00248 //------------------------------------------------------------------------------------------- 00249 00250 void Madgwick::computeAngles() 00251 { 00252 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2); 00253 pitch = asinf(-2.0f * (q1*q3 - q0*q2)); 00254 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); 00255 anglesComputed = 1; 00256 }
Generated on Sun Jul 17 2022 16:11:38 by 1.7.2