Christopher Pollard / Mbed 2 deprecated BLE_IMU_Visualization

Visualize IMU data sent over BLE on a computer

Dependencies:   PinDetect mbed

Fork of LSM9DS1_Library by jim hamblen

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?

UserRevisionLine numberNew 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 }