Madgewick Filter to get attitude from IMU or MARG. Confirmed IMU ver works well.
Dependents: NineIMUAttitude_MadgwickFilter
Diff: MadgwickFilter.hpp
- Revision:
- 5:1e6fecaea25e
- Parent:
- 2:e1de76e257f6
- Child:
- 6:eff5ebc4ea13
diff -r e1de76e257f6 -r 1e6fecaea25e MadgwickFilter.hpp --- a/MadgwickFilter.hpp Sat Jan 28 21:04:15 2017 +0000 +++ b/MadgwickFilter.hpp Sat Dec 16 02:41:00 2017 +0000 @@ -108,6 +108,8 @@ // AHRS algorithm update inline void MadgwickFilter::MadgwickAHRSupdate(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz) { + + double acc_norm; static double deltaT = 0; static unsigned int newTime = 0, oldTime = 0; double recipNorm; @@ -132,7 +134,8 @@ if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement - recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az); + acc_norm = sqrt(ax * ax + ay * ay + az * az); + recipNorm = 1.0 / acc_norm; ax *= recipNorm; ay *= recipNorm; az *= recipNorm; @@ -184,6 +187,13 @@ s2 *= recipNorm; s3 *= recipNorm; + double deltaA = fabs(acc_norm - 1.00); + //beta = 0.1*exp(-1.0*deltaA*deltaA); + //beta = 0.3*exp(-20.0*deltaA*deltaA); + beta = beta*exp(-30.0*deltaA*deltaA); + //printf("%f\r\n", beta); + beta = 1.0; + //if(deltaA > 0.3) beta = 0.0; // Apply feedback step qDot1 -= beta * s0; qDot2 -= beta * s1; @@ -225,6 +235,7 @@ double s0, s1, s2, s3; double qDot1, qDot2, qDot3, qDot4; double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + double acc_norm; // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); @@ -236,7 +247,8 @@ if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement - recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az); + acc_norm = sqrt(ax * ax + ay * ay + az * az); + recipNorm = 1.0 / acc_norm; ax *= recipNorm; ay *= recipNorm; az *= recipNorm; @@ -268,6 +280,10 @@ s3 *= recipNorm; // Apply feedback step + double deltaA = fabs(acc_norm - 1.00); + //beta = 0.5*exp(-20.0*deltaA*deltaA); + if(deltaA > 0.3) beta = 0.0; + else beta = 0.1; qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2;