Madgewick Filter to get attitude from IMU or MARG. Confirmed IMU ver works well.
Dependents: NineIMUAttitude_MadgwickFilter
Diff: MadgwickFilter.hpp
- Revision:
- 6:eff5ebc4ea13
- Parent:
- 5:1e6fecaea25e
- Child:
- 7:c20656d96585
--- a/MadgwickFilter.hpp Sat Dec 16 02:41:00 2017 +0000 +++ b/MadgwickFilter.hpp Sun Feb 25 19:45:22 2018 +0000 @@ -18,7 +18,7 @@ @param B double型, この値を大きくすると重力の影響を大きく取るようになります. @note 引数無しの場合,B = 0.1fが代入されます. */ - MadgwickFilter(double B = BETA_DEF); + MadgwickFilter(float B = BETA_DEF); public: /** @@ -29,7 +29,7 @@ @note 角速度は[rad]にしてください.この関数は出来るだけ高速に呼び出し続けた方が良いと思います. @note 外部でローパスフィルタなどをかけることをお勧めします. */ - void MadgwickAHRSupdate(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz); + void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); /** @bref MadgwickFilterを角速度と加速度のみで動かし,姿勢計算を更新します. @@ -37,7 +37,7 @@ @param ax,ay,az 加速度データ, 特に規格化は必要ありません @note 通常の関数でも,地磁気成分を0.0にすればこの関数が呼ばれます. */ - void MadgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az); + void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az); /** @bref 姿勢を四元数で取得します. @@ -54,22 +54,22 @@ @param _q3 虚部k, double型, アドレス @note unityに入れる際は軸方向を修正してください. */ - void getAttitude(double *_q0, double *_q1, double *_q2, double *_q3); + void getAttitude(float *_q0, float *_q1, float *_q2, float *_q3); /** @bref オイラー角で姿勢を取得します. @param val ロール,ピッチ,ヨーの順に配列に格納します.3つ以上の要素の配列を入れてください. @note 値は[rad]です.[degree]に変換が必要な場合は別途計算して下さい. */ - void getEulerAngle(double *val); + void getEulerAngle(float *val); public: Timer madgwickTimer; Quaternion q; - double q0,q1,q2,q3; - double beta; + float q0,q1,q2,q3; + float beta; }; -MadgwickFilter::MadgwickFilter(double B){ +MadgwickFilter::MadgwickFilter(float B){ q.w = 1.0f; q.x = 0.0f; q.y = 0.0f; @@ -89,7 +89,7 @@ -void MadgwickFilter::getAttitude(double *_q0, double *_q1, double *_q2, double *_q3){ +void MadgwickFilter::getAttitude(float *_q0, float *_q1, float *_q2, float *_q3){ *_q0 = q0; *_q1 = q1; *_q2 = q2; @@ -98,8 +98,8 @@ } -void MadgwickFilter::getEulerAngle(double *val){ - double q0q0 = q0 * q0, q1q1q2q2 = q1 * q1 - q2 * q2, q3q3 = q3 * q3; +void MadgwickFilter::getEulerAngle(float *val){ + float q0q0 = q0 * q0, q1q1q2q2 = q1 * q1 - q2 * q2, q3q3 = q3 * q3; val[0] = (atan2(2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1q2q2 + q3q3)); val[1] = (-asin(2.0f * (q1 * q3 - q0 * q2))); val[2] = (atan2(2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1q2q2 - q3q3)); @@ -107,16 +107,16 @@ //--------------------------------------------------------------------------------------------------- // 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) { +inline void MadgwickFilter::MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { - double acc_norm; - static double deltaT = 0; + static float acc_norm; + static float deltaT = 0.0f; static unsigned int newTime = 0, oldTime = 0; - double recipNorm; - double s0, s1, s2, s3; - double qDot1, qDot2, qDot3, qDot4; - double hx, hy; - double _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + static float recipNorm; + static float s0, s1, s2, s3; + static float qDot1, qDot2, qDot3, qDot4; + static float hx, hy; + static float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { @@ -135,13 +135,13 @@ // Normalise accelerometer measurement acc_norm = sqrt(ax * ax + ay * ay + az * az); - recipNorm = 1.0 / acc_norm; + recipNorm = 1.0f / acc_norm; ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalise magnetometer measurement - recipNorm = 1.0 / sqrt(mx * mx + my * my + mz * mz); + recipNorm = 1.0f / sqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; @@ -181,18 +181,18 @@ s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + recipNorm = 1.0f / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; - double deltaA = fabs(acc_norm - 1.00); + float deltaA = fabs(acc_norm - 1.00f); //beta = 0.1*exp(-1.0*deltaA*deltaA); //beta = 0.3*exp(-20.0*deltaA*deltaA); - beta = beta*exp(-30.0*deltaA*deltaA); + beta = beta*exp(-30.0f*deltaA*deltaA); //printf("%f\r\n", beta); - beta = 1.0; + //beta = 1.0; //if(deltaA > 0.3) beta = 0.0; // Apply feedback step qDot1 -= beta * s0; @@ -203,7 +203,7 @@ // Integrate rate of change of quaternion to yield quaternion newTime = (unsigned int)madgwickTimer.read_us(); - deltaT = (newTime - oldTime) / 1000000.0; + deltaT = (newTime - oldTime) / 1000000.0f; deltaT = fabs(deltaT); oldTime = newTime; @@ -213,7 +213,7 @@ q3 += qDot4 * deltaT;//(1.0f / sampleFreq); // Normalise quaternion - recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + recipNorm = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; @@ -228,14 +228,14 @@ //--------------------------------------------------------------------------------------------------- // IMU algorithm update -inline void MadgwickFilter::MadgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az) { - static double deltaT = 0; +inline void MadgwickFilter::MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { + static float deltaT = 0; static unsigned int newTime = 0, oldTime = 0; - double recipNorm; - 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; + static float recipNorm; + static float s0, s1, s2, s3; + static float qDot1, qDot2, qDot3, qDot4; + static float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + static float acc_norm; // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); @@ -248,7 +248,7 @@ // Normalise accelerometer measurement acc_norm = sqrt(ax * ax + ay * ay + az * az); - recipNorm = 1.0 / acc_norm; + recipNorm = 1.0f / acc_norm; ax *= recipNorm; ay *= recipNorm; az *= recipNorm; @@ -273,17 +273,18 @@ s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + recipNorm = 1.0f / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; // Apply feedback step - double deltaA = fabs(acc_norm - 1.00); + static float deltaA; + deltaA = fabs(acc_norm - 1.00f); //beta = 0.5*exp(-20.0*deltaA*deltaA); - if(deltaA > 0.3) beta = 0.0; - else beta = 0.1; + if(deltaA > 0.3f) beta = 0.0f; + else beta = 0.1f; qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2; @@ -292,7 +293,7 @@ // Integrate rate of change of quaternion to yield quaternion newTime = (unsigned int)madgwickTimer.read_us(); - deltaT = (newTime - oldTime) / 1000000.0; + deltaT = (newTime - oldTime) / 1000000.0f; deltaT = fabs(deltaT); oldTime = newTime; q0 += qDot1 * deltaT;; @@ -301,7 +302,7 @@ q3 += qDot4 * deltaT;; // Normalise quaternion - recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + recipNorm = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm;