Visualize IMU data sent over BLE on a computer
Fork of LSM9DS1_Library by
MadgwickUpdate.h@3:fd549671e512, 2016-03-13 (annotated)
- Committer:
- cvmp94
- Date:
- Sun Mar 13 21:40:10 2016 +0000
- Revision:
- 3:fd549671e512
initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cvmp94 | 3:fd549671e512 | 1 | #include "mbed.h" |
cvmp94 | 3:fd549671e512 | 2 | #include "math.h" |
cvmp94 | 3:fd549671e512 | 3 | |
cvmp94 | 3:fd549671e512 | 4 | float deltat = 0.0f; |
cvmp94 | 3:fd549671e512 | 5 | int lastUpdate = 0, firstUpdate = 0, Now = 0; |
cvmp94 | 3:fd549671e512 | 6 | float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; |
cvmp94 | 3:fd549671e512 | 7 | |
cvmp94 | 3:fd549671e512 | 8 | float PI = 3.14159265358979323846f; |
cvmp94 | 3:fd549671e512 | 9 | float GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 |
cvmp94 | 3:fd549671e512 | 10 | float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta |
cvmp94 | 3:fd549671e512 | 11 | float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) |
cvmp94 | 3:fd549671e512 | 12 | float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; |
cvmp94 | 3:fd549671e512 | 13 | |
cvmp94 | 3:fd549671e512 | 14 | void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz) { |
cvmp94 | 3:fd549671e512 | 15 | float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability |
cvmp94 | 3:fd549671e512 | 16 | float norm; // vector norm |
cvmp94 | 3:fd549671e512 | 17 | float f1, f2, f3; // objective funcyion elements |
cvmp94 | 3:fd549671e512 | 18 | float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements |
cvmp94 | 3:fd549671e512 | 19 | float qDot1, qDot2, qDot3, qDot4; |
cvmp94 | 3:fd549671e512 | 20 | float hatDot1, hatDot2, hatDot3, hatDot4; |
cvmp94 | 3:fd549671e512 | 21 | float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz; // gyro bias error |
cvmp94 | 3:fd549671e512 | 22 | |
cvmp94 | 3:fd549671e512 | 23 | // Auxiliary variables to avoid repeated arithmetic |
cvmp94 | 3:fd549671e512 | 24 | float _halfq1 = 0.5f * q1; |
cvmp94 | 3:fd549671e512 | 25 | float _halfq2 = 0.5f * q2; |
cvmp94 | 3:fd549671e512 | 26 | float _halfq3 = 0.5f * q3; |
cvmp94 | 3:fd549671e512 | 27 | float _halfq4 = 0.5f * q4; |
cvmp94 | 3:fd549671e512 | 28 | float _2q1 = 2.0f * q1; |
cvmp94 | 3:fd549671e512 | 29 | float _2q2 = 2.0f * q2; |
cvmp94 | 3:fd549671e512 | 30 | float _2q3 = 2.0f * q3; |
cvmp94 | 3:fd549671e512 | 31 | float _2q4 = 2.0f * q4; |
cvmp94 | 3:fd549671e512 | 32 | |
cvmp94 | 3:fd549671e512 | 33 | // Normalise accelerometer measurement |
cvmp94 | 3:fd549671e512 | 34 | norm = sqrt(ax * ax + ay * ay + az * az); |
cvmp94 | 3:fd549671e512 | 35 | if (norm == 0.0f) return; // handle NaN |
cvmp94 | 3:fd549671e512 | 36 | norm = 1.0f/norm; |
cvmp94 | 3:fd549671e512 | 37 | ax *= norm; |
cvmp94 | 3:fd549671e512 | 38 | ay *= norm; |
cvmp94 | 3:fd549671e512 | 39 | az *= norm; |
cvmp94 | 3:fd549671e512 | 40 | |
cvmp94 | 3:fd549671e512 | 41 | // Compute the objective function and Jacobian |
cvmp94 | 3:fd549671e512 | 42 | f1 = _2q2 * q4 - _2q1 * q3 - ax; |
cvmp94 | 3:fd549671e512 | 43 | f2 = _2q1 * q2 + _2q3 * q4 - ay; |
cvmp94 | 3:fd549671e512 | 44 | f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az; |
cvmp94 | 3:fd549671e512 | 45 | J_11or24 = _2q3; |
cvmp94 | 3:fd549671e512 | 46 | J_12or23 = _2q4; |
cvmp94 | 3:fd549671e512 | 47 | J_13or22 = _2q1; |
cvmp94 | 3:fd549671e512 | 48 | J_14or21 = _2q2; |
cvmp94 | 3:fd549671e512 | 49 | J_32 = 2.0f * J_14or21; |
cvmp94 | 3:fd549671e512 | 50 | J_33 = 2.0f * J_11or24; |
cvmp94 | 3:fd549671e512 | 51 | |
cvmp94 | 3:fd549671e512 | 52 | // Compute the gradient (matrix multiplication) |
cvmp94 | 3:fd549671e512 | 53 | hatDot1 = J_14or21 * f2 - J_11or24 * f1; |
cvmp94 | 3:fd549671e512 | 54 | hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3; |
cvmp94 | 3:fd549671e512 | 55 | hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1; |
cvmp94 | 3:fd549671e512 | 56 | hatDot4 = J_14or21 * f1 + J_11or24 * f2; |
cvmp94 | 3:fd549671e512 | 57 | |
cvmp94 | 3:fd549671e512 | 58 | // Normalize the gradient |
cvmp94 | 3:fd549671e512 | 59 | norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4); |
cvmp94 | 3:fd549671e512 | 60 | hatDot1 /= norm; |
cvmp94 | 3:fd549671e512 | 61 | hatDot2 /= norm; |
cvmp94 | 3:fd549671e512 | 62 | hatDot3 /= norm; |
cvmp94 | 3:fd549671e512 | 63 | hatDot4 /= norm; |
cvmp94 | 3:fd549671e512 | 64 | |
cvmp94 | 3:fd549671e512 | 65 | // Compute estimated gyroscope biases |
cvmp94 | 3:fd549671e512 | 66 | gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3; |
cvmp94 | 3:fd549671e512 | 67 | gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2; |
cvmp94 | 3:fd549671e512 | 68 | gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1; |
cvmp94 | 3:fd549671e512 | 69 | |
cvmp94 | 3:fd549671e512 | 70 | // Compute and remove gyroscope biases |
cvmp94 | 3:fd549671e512 | 71 | gbiasx += gerrx * deltat * zeta; |
cvmp94 | 3:fd549671e512 | 72 | gbiasy += gerry * deltat * zeta; |
cvmp94 | 3:fd549671e512 | 73 | gbiasz += gerrz * deltat * zeta; |
cvmp94 | 3:fd549671e512 | 74 | // gx -= gbiasx; |
cvmp94 | 3:fd549671e512 | 75 | // gy -= gbiasy; |
cvmp94 | 3:fd549671e512 | 76 | // gz -= gbiasz; |
cvmp94 | 3:fd549671e512 | 77 | |
cvmp94 | 3:fd549671e512 | 78 | // Compute the quaternion derivative |
cvmp94 | 3:fd549671e512 | 79 | qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz; |
cvmp94 | 3:fd549671e512 | 80 | qDot2 = _halfq1 * gx + _halfq3 * gz - _halfq4 * gy; |
cvmp94 | 3:fd549671e512 | 81 | qDot3 = _halfq1 * gy - _halfq2 * gz + _halfq4 * gx; |
cvmp94 | 3:fd549671e512 | 82 | qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx; |
cvmp94 | 3:fd549671e512 | 83 | |
cvmp94 | 3:fd549671e512 | 84 | // Compute then integrate estimated quaternion derivative |
cvmp94 | 3:fd549671e512 | 85 | q1 += (qDot1 -(beta * hatDot1)) * deltat; |
cvmp94 | 3:fd549671e512 | 86 | q2 += (qDot2 -(beta * hatDot2)) * deltat; |
cvmp94 | 3:fd549671e512 | 87 | q3 += (qDot3 -(beta * hatDot3)) * deltat; |
cvmp94 | 3:fd549671e512 | 88 | q4 += (qDot4 -(beta * hatDot4)) * deltat; |
cvmp94 | 3:fd549671e512 | 89 | |
cvmp94 | 3:fd549671e512 | 90 | // Normalize the quaternion |
cvmp94 | 3:fd549671e512 | 91 | norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion |
cvmp94 | 3:fd549671e512 | 92 | norm = 1.0f/norm; |
cvmp94 | 3:fd549671e512 | 93 | q[0] = q1 * norm; |
cvmp94 | 3:fd549671e512 | 94 | q[1] = q2 * norm; |
cvmp94 | 3:fd549671e512 | 95 | q[2] = q3 * norm; |
cvmp94 | 3:fd549671e512 | 96 | q[3] = q4 * norm; |
cvmp94 | 3:fd549671e512 | 97 | |
cvmp94 | 3:fd549671e512 | 98 | } |