altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Mahony.cpp@1:36bbe04e1f6f, 2018-10-26 (annotated)

Committer:
altb
Date:
Fri Oct 26 05:56:25 2018 +0000
Revision:
1:36bbe04e1f6f
Child:
3:6811c0ce95f6
Added Mahony Class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 1:36bbe04e1f6f 1 //=============================================================================================
altb 1:36bbe04e1f6f 2 // Mahony.c
altb 1:36bbe04e1f6f 3 //=============================================================================================
altb 1:36bbe04e1f6f 4 //
altb 1:36bbe04e1f6f 5 // Madgwick's implementation of Mayhony's AHRS algorithm.
altb 1:36bbe04e1f6f 6 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
altb 1:36bbe04e1f6f 7 //
altb 1:36bbe04e1f6f 8 // From the x-io website "Open-source resources available on this website are
altb 1:36bbe04e1f6f 9 // provided under the GNU General Public Licence unless an alternative licence
altb 1:36bbe04e1f6f 10 // is provided in source."
altb 1:36bbe04e1f6f 11 //
altb 1:36bbe04e1f6f 12 // Date Author Notes
altb 1:36bbe04e1f6f 13 // 29/09/2011 SOH Madgwick Initial release
altb 1:36bbe04e1f6f 14 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
altb 1:36bbe04e1f6f 15 //
altb 1:36bbe04e1f6f 16 // Algorithm paper:
altb 1:36bbe04e1f6f 17 // http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
altb 1:36bbe04e1f6f 18 //
altb 1:36bbe04e1f6f 19 //=============================================================================================
altb 1:36bbe04e1f6f 20
altb 1:36bbe04e1f6f 21 //-------------------------------------------------------------------------------------------
altb 1:36bbe04e1f6f 22 // Header files
altb 1:36bbe04e1f6f 23
altb 1:36bbe04e1f6f 24 #include "Mahony.h"
altb 1:36bbe04e1f6f 25 #include <math.h>
altb 1:36bbe04e1f6f 26
altb 1:36bbe04e1f6f 27 //-------------------------------------------------------------------------------------------
altb 1:36bbe04e1f6f 28 // Definitions
altb 1:36bbe04e1f6f 29
altb 1:36bbe04e1f6f 30 #define DEFAULT_SAMPLE_FREQ 100.0f // sample frequency in Hz
altb 1:36bbe04e1f6f 31 #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
altb 1:36bbe04e1f6f 32 #define twoKiDef (2.0f * 0.0f) // 2 * integral gain
altb 1:36bbe04e1f6f 33
altb 1:36bbe04e1f6f 34
altb 1:36bbe04e1f6f 35 //============================================================================================
altb 1:36bbe04e1f6f 36 // Functions
altb 1:36bbe04e1f6f 37
altb 1:36bbe04e1f6f 38 //-------------------------------------------------------------------------------------------
altb 1:36bbe04e1f6f 39 // AHRS algorithm update
altb 1:36bbe04e1f6f 40
altb 1:36bbe04e1f6f 41 Mahony::Mahony()
altb 1:36bbe04e1f6f 42 {
altb 1:36bbe04e1f6f 43 twoKp = twoKpDef; // 2 * proportional gain (Kp)
altb 1:36bbe04e1f6f 44 twoKi = twoKiDef; // 2 * integral gain (Ki)
altb 1:36bbe04e1f6f 45 q0 = 1.0f;
altb 1:36bbe04e1f6f 46 q1 = 0.0f;
altb 1:36bbe04e1f6f 47 q2 = 0.0f;
altb 1:36bbe04e1f6f 48 q3 = 0.0f;
altb 1:36bbe04e1f6f 49 integralFBx = 0.0f;
altb 1:36bbe04e1f6f 50 integralFBy = 0.0f;
altb 1:36bbe04e1f6f 51 integralFBz = 0.0f;
altb 1:36bbe04e1f6f 52 anglesComputed = 0;
altb 1:36bbe04e1f6f 53 invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
altb 1:36bbe04e1f6f 54 }
altb 1:36bbe04e1f6f 55
altb 1:36bbe04e1f6f 56 void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
altb 1:36bbe04e1f6f 57 {
altb 1:36bbe04e1f6f 58 float recipNorm;
altb 1:36bbe04e1f6f 59 float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
altb 1:36bbe04e1f6f 60 float hx, hy, bx, bz;
altb 1:36bbe04e1f6f 61 float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
altb 1:36bbe04e1f6f 62 float halfex, halfey, halfez;
altb 1:36bbe04e1f6f 63 float qa, qb, qc;
altb 1:36bbe04e1f6f 64
altb 1:36bbe04e1f6f 65 // Use IMU algorithm if magnetometer measurement invalid
altb 1:36bbe04e1f6f 66 // (avoids NaN in magnetometer normalisation)
altb 1:36bbe04e1f6f 67 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
altb 1:36bbe04e1f6f 68 updateIMU(gx, gy, gz, ax, ay, az);
altb 1:36bbe04e1f6f 69 return;
altb 1:36bbe04e1f6f 70 }
altb 1:36bbe04e1f6f 71
altb 1:36bbe04e1f6f 72 // Convert gyroscope degrees/sec to radians/sec
altb 1:36bbe04e1f6f 73 // gx *= 0.0174533f;
altb 1:36bbe04e1f6f 74 // gy *= 0.0174533f;
altb 1:36bbe04e1f6f 75 // gz *= 0.0174533f;
altb 1:36bbe04e1f6f 76
altb 1:36bbe04e1f6f 77 // Compute feedback only if accelerometer measurement valid
altb 1:36bbe04e1f6f 78 // (avoids NaN in accelerometer normalisation)
altb 1:36bbe04e1f6f 79 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
altb 1:36bbe04e1f6f 80
altb 1:36bbe04e1f6f 81 // Normalise accelerometer measurement
altb 1:36bbe04e1f6f 82 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
altb 1:36bbe04e1f6f 83 ax *= recipNorm;
altb 1:36bbe04e1f6f 84 ay *= recipNorm;
altb 1:36bbe04e1f6f 85 az *= recipNorm;
altb 1:36bbe04e1f6f 86
altb 1:36bbe04e1f6f 87 // Normalise magnetometer measurement
altb 1:36bbe04e1f6f 88 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
altb 1:36bbe04e1f6f 89 mx *= recipNorm;
altb 1:36bbe04e1f6f 90 my *= recipNorm;
altb 1:36bbe04e1f6f 91 mz *= recipNorm;
altb 1:36bbe04e1f6f 92
altb 1:36bbe04e1f6f 93 // Auxiliary variables to avoid repeated arithmetic
altb 1:36bbe04e1f6f 94 q0q0 = q0 * q0;
altb 1:36bbe04e1f6f 95 q0q1 = q0 * q1;
altb 1:36bbe04e1f6f 96 q0q2 = q0 * q2;
altb 1:36bbe04e1f6f 97 q0q3 = q0 * q3;
altb 1:36bbe04e1f6f 98 q1q1 = q1 * q1;
altb 1:36bbe04e1f6f 99 q1q2 = q1 * q2;
altb 1:36bbe04e1f6f 100 q1q3 = q1 * q3;
altb 1:36bbe04e1f6f 101 q2q2 = q2 * q2;
altb 1:36bbe04e1f6f 102 q2q3 = q2 * q3;
altb 1:36bbe04e1f6f 103 q3q3 = q3 * q3;
altb 1:36bbe04e1f6f 104
altb 1:36bbe04e1f6f 105 // Reference direction of Earth's magnetic field
altb 1:36bbe04e1f6f 106 hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
altb 1:36bbe04e1f6f 107 hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
altb 1:36bbe04e1f6f 108 bx = sqrtf(hx * hx + hy * hy);
altb 1:36bbe04e1f6f 109 bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
altb 1:36bbe04e1f6f 110
altb 1:36bbe04e1f6f 111 // Estimated direction of gravity and magnetic field
altb 1:36bbe04e1f6f 112 halfvx = q1q3 - q0q2;
altb 1:36bbe04e1f6f 113 halfvy = q0q1 + q2q3;
altb 1:36bbe04e1f6f 114 halfvz = q0q0 - 0.5f + q3q3;
altb 1:36bbe04e1f6f 115 halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
altb 1:36bbe04e1f6f 116 halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
altb 1:36bbe04e1f6f 117 halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
altb 1:36bbe04e1f6f 118
altb 1:36bbe04e1f6f 119 // Error is sum of cross product between estimated direction
altb 1:36bbe04e1f6f 120 // and measured direction of field vectors
altb 1:36bbe04e1f6f 121 halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
altb 1:36bbe04e1f6f 122 halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
altb 1:36bbe04e1f6f 123 halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
altb 1:36bbe04e1f6f 124
altb 1:36bbe04e1f6f 125 // Compute and apply integral feedback if enabled
altb 1:36bbe04e1f6f 126 if(twoKi > 0.0f) {
altb 1:36bbe04e1f6f 127 // integral error scaled by Ki
altb 1:36bbe04e1f6f 128 integralFBx += twoKi * halfex * invSampleFreq;
altb 1:36bbe04e1f6f 129 integralFBy += twoKi * halfey * invSampleFreq;
altb 1:36bbe04e1f6f 130 integralFBz += twoKi * halfez * invSampleFreq;
altb 1:36bbe04e1f6f 131 gx += integralFBx; // apply integral feedback
altb 1:36bbe04e1f6f 132 gy += integralFBy;
altb 1:36bbe04e1f6f 133 gz += integralFBz;
altb 1:36bbe04e1f6f 134 } else {
altb 1:36bbe04e1f6f 135 integralFBx = 0.0f; // prevent integral windup
altb 1:36bbe04e1f6f 136 integralFBy = 0.0f;
altb 1:36bbe04e1f6f 137 integralFBz = 0.0f;
altb 1:36bbe04e1f6f 138 }
altb 1:36bbe04e1f6f 139
altb 1:36bbe04e1f6f 140 // Apply proportional feedback
altb 1:36bbe04e1f6f 141 gx += twoKp * halfex;
altb 1:36bbe04e1f6f 142 gy += twoKp * halfey;
altb 1:36bbe04e1f6f 143 gz += twoKp * halfez;
altb 1:36bbe04e1f6f 144 }
altb 1:36bbe04e1f6f 145
altb 1:36bbe04e1f6f 146 // Integrate rate of change of quaternion
altb 1:36bbe04e1f6f 147 gx *= (0.5f * invSampleFreq); // pre-multiply common factors
altb 1:36bbe04e1f6f 148 gy *= (0.5f * invSampleFreq);
altb 1:36bbe04e1f6f 149 gz *= (0.5f * invSampleFreq);
altb 1:36bbe04e1f6f 150 qa = q0;
altb 1:36bbe04e1f6f 151 qb = q1;
altb 1:36bbe04e1f6f 152 qc = q2;
altb 1:36bbe04e1f6f 153 q0 += (-qb * gx - qc * gy - q3 * gz);
altb 1:36bbe04e1f6f 154 q1 += (qa * gx + qc * gz - q3 * gy);
altb 1:36bbe04e1f6f 155 q2 += (qa * gy - qb * gz + q3 * gx);
altb 1:36bbe04e1f6f 156 q3 += (qa * gz + qb * gy - qc * gx);
altb 1:36bbe04e1f6f 157
altb 1:36bbe04e1f6f 158 // Normalise quaternion
altb 1:36bbe04e1f6f 159 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
altb 1:36bbe04e1f6f 160 q0 *= recipNorm;
altb 1:36bbe04e1f6f 161 q1 *= recipNorm;
altb 1:36bbe04e1f6f 162 q2 *= recipNorm;
altb 1:36bbe04e1f6f 163 q3 *= recipNorm;
altb 1:36bbe04e1f6f 164 anglesComputed = 0;
altb 1:36bbe04e1f6f 165 }
altb 1:36bbe04e1f6f 166
altb 1:36bbe04e1f6f 167 //-------------------------------------------------------------------------------------------
altb 1:36bbe04e1f6f 168 // IMU algorithm update
altb 1:36bbe04e1f6f 169
altb 1:36bbe04e1f6f 170 void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
altb 1:36bbe04e1f6f 171 {
altb 1:36bbe04e1f6f 172 float recipNorm;
altb 1:36bbe04e1f6f 173 float halfvx, halfvy, halfvz;
altb 1:36bbe04e1f6f 174 float halfex, halfey, halfez;
altb 1:36bbe04e1f6f 175 float qa, qb, qc;
altb 1:36bbe04e1f6f 176
altb 1:36bbe04e1f6f 177 // Convert gyroscope degrees/sec to radians/sec
altb 1:36bbe04e1f6f 178 // gx *= 0.0174533f;
altb 1:36bbe04e1f6f 179 // gy *= 0.0174533f;
altb 1:36bbe04e1f6f 180 // gz *= 0.0174533f;
altb 1:36bbe04e1f6f 181
altb 1:36bbe04e1f6f 182 // Compute feedback only if accelerometer measurement valid
altb 1:36bbe04e1f6f 183 // (avoids NaN in accelerometer normalisation)
altb 1:36bbe04e1f6f 184 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
altb 1:36bbe04e1f6f 185
altb 1:36bbe04e1f6f 186 // Normalise accelerometer measurement
altb 1:36bbe04e1f6f 187 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
altb 1:36bbe04e1f6f 188 ax *= recipNorm;
altb 1:36bbe04e1f6f 189 ay *= recipNorm;
altb 1:36bbe04e1f6f 190 az *= recipNorm;
altb 1:36bbe04e1f6f 191
altb 1:36bbe04e1f6f 192 // Estimated direction of gravity
altb 1:36bbe04e1f6f 193 halfvx = q1 * q3 - q0 * q2;
altb 1:36bbe04e1f6f 194 halfvy = q0 * q1 + q2 * q3;
altb 1:36bbe04e1f6f 195 halfvz = q0 * q0 - 0.5f + q3 * q3;
altb 1:36bbe04e1f6f 196
altb 1:36bbe04e1f6f 197 // Error is sum of cross product between estimated
altb 1:36bbe04e1f6f 198 // and measured direction of gravity
altb 1:36bbe04e1f6f 199 halfex = (ay * halfvz - az * halfvy);
altb 1:36bbe04e1f6f 200 halfey = (az * halfvx - ax * halfvz);
altb 1:36bbe04e1f6f 201 halfez = (ax * halfvy - ay * halfvx);
altb 1:36bbe04e1f6f 202
altb 1:36bbe04e1f6f 203 // Compute and apply integral feedback if enabled
altb 1:36bbe04e1f6f 204 if(twoKi > 0.0f) {
altb 1:36bbe04e1f6f 205 // integral error scaled by Ki
altb 1:36bbe04e1f6f 206 integralFBx += twoKi * halfex * invSampleFreq;
altb 1:36bbe04e1f6f 207 integralFBy += twoKi * halfey * invSampleFreq;
altb 1:36bbe04e1f6f 208 integralFBz += twoKi * halfez * invSampleFreq;
altb 1:36bbe04e1f6f 209 gx += integralFBx; // apply integral feedback
altb 1:36bbe04e1f6f 210 gy += integralFBy;
altb 1:36bbe04e1f6f 211 gz += integralFBz;
altb 1:36bbe04e1f6f 212 } else {
altb 1:36bbe04e1f6f 213 integralFBx = 0.0f; // prevent integral windup
altb 1:36bbe04e1f6f 214 integralFBy = 0.0f;
altb 1:36bbe04e1f6f 215 integralFBz = 0.0f;
altb 1:36bbe04e1f6f 216 }
altb 1:36bbe04e1f6f 217
altb 1:36bbe04e1f6f 218 // Apply proportional feedback
altb 1:36bbe04e1f6f 219 gx += twoKp * halfex;
altb 1:36bbe04e1f6f 220 gy += twoKp * halfey;
altb 1:36bbe04e1f6f 221 gz += twoKp * halfez;
altb 1:36bbe04e1f6f 222 }
altb 1:36bbe04e1f6f 223
altb 1:36bbe04e1f6f 224 // Integrate rate of change of quaternion
altb 1:36bbe04e1f6f 225 gx *= (0.5f * invSampleFreq); // pre-multiply common factors
altb 1:36bbe04e1f6f 226 gy *= (0.5f * invSampleFreq);
altb 1:36bbe04e1f6f 227 gz *= (0.5f * invSampleFreq);
altb 1:36bbe04e1f6f 228 qa = q0;
altb 1:36bbe04e1f6f 229 qb = q1;
altb 1:36bbe04e1f6f 230 qc = q2;
altb 1:36bbe04e1f6f 231 q0 += (-qb * gx - qc * gy - q3 * gz);
altb 1:36bbe04e1f6f 232 q1 += (qa * gx + qc * gz - q3 * gy);
altb 1:36bbe04e1f6f 233 q2 += (qa * gy - qb * gz + q3 * gx);
altb 1:36bbe04e1f6f 234 q3 += (qa * gz + qb * gy - qc * gx);
altb 1:36bbe04e1f6f 235
altb 1:36bbe04e1f6f 236 // Normalise quaternion
altb 1:36bbe04e1f6f 237 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
altb 1:36bbe04e1f6f 238 q0 *= recipNorm;
altb 1:36bbe04e1f6f 239 q1 *= recipNorm;
altb 1:36bbe04e1f6f 240 q2 *= recipNorm;
altb 1:36bbe04e1f6f 241 q3 *= recipNorm;
altb 1:36bbe04e1f6f 242 anglesComputed = 0;
altb 1:36bbe04e1f6f 243 }
altb 1:36bbe04e1f6f 244
altb 1:36bbe04e1f6f 245 //-------------------------------------------------------------------------------------------
altb 1:36bbe04e1f6f 246 // Fast inverse square-root
altb 1:36bbe04e1f6f 247 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
altb 1:36bbe04e1f6f 248
altb 1:36bbe04e1f6f 249 float Mahony::invSqrt(float x)
altb 1:36bbe04e1f6f 250 {
altb 1:36bbe04e1f6f 251 float halfx = 0.5f * x;
altb 1:36bbe04e1f6f 252 float y = x;
altb 1:36bbe04e1f6f 253 long i = *(long*)&y;
altb 1:36bbe04e1f6f 254 i = 0x5f3759df - (i>>1);
altb 1:36bbe04e1f6f 255 y = *(float*)&i;
altb 1:36bbe04e1f6f 256 y = y * (1.5f - (halfx * y * y));
altb 1:36bbe04e1f6f 257 y = y * (1.5f - (halfx * y * y));
altb 1:36bbe04e1f6f 258 return y;
altb 1:36bbe04e1f6f 259 }
altb 1:36bbe04e1f6f 260
altb 1:36bbe04e1f6f 261 //-------------------------------------------------------------------------------------------
altb 1:36bbe04e1f6f 262
altb 1:36bbe04e1f6f 263 void Mahony::computeAngles()
altb 1:36bbe04e1f6f 264 {
altb 1:36bbe04e1f6f 265 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
altb 1:36bbe04e1f6f 266 pitch = asinf(-2.0f * (q1*q3 - q0*q2));
altb 1:36bbe04e1f6f 267 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
altb 1:36bbe04e1f6f 268 anglesComputed = 1;
altb 1:36bbe04e1f6f 269 }
altb 1:36bbe04e1f6f 270
altb 1:36bbe04e1f6f 271
altb 1:36bbe04e1f6f 272 //============================================================================================
altb 1:36bbe04e1f6f 273 // END OF CODE
altb 1:36bbe04e1f6f 274 //============================================================================================
altb 1:36bbe04e1f6f 275