MadgwickUpdate.h

00001 #include "mbed.h"
00002 #include "math.h"
00003 
00004 float deltat = 0.0f;
00005 int lastUpdate = 0, firstUpdate = 0, Now = 0;
00006 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
00007 
00008 float PI = 3.14159265358979323846f;
00009 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
00010 float beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
00011 float GyroMeasDrift = PI * (1.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
00012 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
00013 
00014 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz) {
00015     float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];         // short name local variable for readability
00016     float norm;                                               // vector norm
00017     float f1, f2, f3;                                         // objective funcyion elements
00018     float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements
00019     float qDot1, qDot2, qDot3, qDot4;
00020     float hatDot1, hatDot2, hatDot3, hatDot4;
00021     float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz;  // gyro bias error
00022 
00023     // Auxiliary variables to avoid repeated arithmetic
00024     float _halfq1 = 0.5f * q1;
00025     float _halfq2 = 0.5f * q2;
00026     float _halfq3 = 0.5f * q3;
00027     float _halfq4 = 0.5f * q4;
00028     float _2q1 = 2.0f * q1;
00029     float _2q2 = 2.0f * q2;
00030     float _2q3 = 2.0f * q3;
00031     float _2q4 = 2.0f * q4;
00032 
00033     // Normalise accelerometer measurement
00034     norm = sqrt(ax * ax + ay * ay + az * az);
00035     if (norm == 0.0f) return; // handle NaN
00036     norm = 1.0f/norm;
00037     ax *= norm;
00038     ay *= norm;
00039     az *= norm;
00040     
00041     // Compute the objective function and Jacobian
00042     f1 = _2q2 * q4 - _2q1 * q3 - ax;
00043     f2 = _2q1 * q2 + _2q3 * q4 - ay;
00044     f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;
00045     J_11or24 = _2q3;
00046     J_12or23 = _2q4;
00047     J_13or22 = _2q1;
00048     J_14or21 = _2q2;
00049     J_32 = 2.0f * J_14or21;
00050     J_33 = 2.0f * J_11or24;
00051   
00052     // Compute the gradient (matrix multiplication)
00053     hatDot1 = J_14or21 * f2 - J_11or24 * f1;
00054     hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;
00055     hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1;
00056     hatDot4 = J_14or21 * f1 + J_11or24 * f2;
00057     
00058     // Normalize the gradient
00059     norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
00060     hatDot1 /= norm;
00061     hatDot2 /= norm;
00062     hatDot3 /= norm;
00063     hatDot4 /= norm;
00064     
00065     // Compute estimated gyroscope biases
00066     gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;
00067     gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;
00068     gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;
00069     
00070     // Compute and remove gyroscope biases
00071     gbiasx += gerrx * deltat * zeta;
00072     gbiasy += gerry * deltat * zeta;
00073     gbiasz += gerrz * deltat * zeta;
00074 //           gx -= gbiasx;
00075 //           gy -= gbiasy;
00076 //           gz -= gbiasz;
00077     
00078     // Compute the quaternion derivative
00079     qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz;
00080     qDot2 =  _halfq1 * gx + _halfq3 * gz - _halfq4 * gy;
00081     qDot3 =  _halfq1 * gy - _halfq2 * gz + _halfq4 * gx;
00082     qDot4 =  _halfq1 * gz + _halfq2 * gy - _halfq3 * gx;
00083 
00084     // Compute then integrate estimated quaternion derivative
00085     q1 += (qDot1 -(beta * hatDot1)) * deltat;
00086     q2 += (qDot2 -(beta * hatDot2)) * deltat;
00087     q3 += (qDot3 -(beta * hatDot3)) * deltat;
00088     q4 += (qDot4 -(beta * hatDot4)) * deltat;
00089 
00090     // Normalize the quaternion
00091     norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
00092     norm = 1.0f/norm;
00093     q[0] = q1 * norm;
00094     q[1] = q2 * norm;
00095     q[2] = q3 * norm;
00096     q[3] = q4 * norm;
00097     
00098 }