Madgewick Filter to get attitude from IMU or MARG. Confirmed IMU ver works well.

Dependencies:   Quaternion

Dependents:   NineIMUAttitude_MadgwickFilter

Files at this revision

API Documentation at this revision

Comitter:
aktk
Date:
Fri Aug 21 03:13:08 2020 +0000
Parent:
7:c20656d96585
Commit message:
Forked and modified some parts

Changed in this revision

MadgwickFilter.hpp Show annotated file Show diff for this revision Revisions of this file
diff -r c20656d96585 -r 72c013425ecc MadgwickFilter.hpp
--- a/MadgwickFilter.hpp	Wed Feb 28 17:10:35 2018 +0000
+++ b/MadgwickFilter.hpp	Fri Aug 21 03:13:08 2020 +0000
@@ -6,20 +6,22 @@
 
 #define BETA_DEF 0.1
 
+
 /**
 *  @bref Madgwick Filterを用いて,角速度・加速度・地磁気データを統合し,姿勢を推定するライブラリです.
 *  @note Quaternion.hppを利用されることをお勧めいたします.
 */
-class MadgwickFilter{
-    
+class MadgwickFilter
+{
+
 public:
     /**
       @bref   マドグウィックフィルター(マッジュウィックフィルター)クラスのコンストラクタ
         @param  B   double型, この値を大きくすると重力の影響を大きく取るようになります.
         @note   引数無しの場合,B = 0.1fが代入されます.
     */
-    MadgwickFilter(float B = BETA_DEF);
-    
+    MadgwickFilter(float B = BETA_DEF, float dT_sec = 0.05f);
+
 public:
     /**
         @bref   MadgwickFilterによって角速度・加速度・地磁気データを統合し,姿勢計算します.
@@ -30,17 +32,17 @@
         @note   外部でローパスフィルタなどをかけることをお勧めします.
     */
     void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
-    
-        /**
-        @bref   MadgwickFilterによって角速度・加速度・地磁気データを統合し,姿勢計算します.
-        @param  gyro    角速度データ,[rad]に変換してから入れてください.
-        @param  acc    加速度データ, 特に規格化は必要ありません
-        @param  mag    地磁気データ, キャリブレーションを確実に行って下さい.
-        @note   角速度は[rad]にしてください.この関数は出来るだけ高速に呼び出し続けた方が良いと思います.
-        @note   外部でローパスフィルタなどをかけることをお勧めします.
+
+    /**
+    @bref   MadgwickFilterによって角速度・加速度・地磁気データを統合し,姿勢計算します.
+    @param  gyro    角速度データ,[rad]に変換してから入れてください.
+    @param  acc    加速度データ, 特に規格化は必要ありません
+    @param  mag    地磁気データ, キャリブレーションを確実に行って下さい.
+    @note   角速度は[rad]にしてください.この関数は出来るだけ高速に呼び出し続けた方が良いと思います.
+    @note   外部でローパスフィルタなどをかけることをお勧めします.
     */
     void MadgwickAHRSupdate(float *gyro, float *acc, float *mag);
-    
+
     /**
         @bref   MadgwickFilterを角速度と加速度のみで動かし,姿勢計算を更新します.
         @param  gx,gy,gz    角速度データ,[rad]に変換してから入れてください.
@@ -48,14 +50,14 @@
         @note   通常の関数でも,地磁気成分を0.0にすればこの関数が呼ばれます.
     */
     void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
-    
+
     /**
         @bref   姿勢を四元数で取得します.
         @param  Q   クォータニオンクラスのインスタンスアドレス, w・i・j・kを更新します.
         @note   unityに入れる際は軸方向を修正してください.
     */
     void getAttitude(Quaternion *Q);
-    
+
     /**
         @bref   姿勢を四元数で取得します.
         @param  _q0 実部w, double型, アドレス
@@ -65,7 +67,7 @@
         @note   unityに入れる際は軸方向を修正してください.
     */
     void getAttitude(float *_q0, float *_q1, float *_q2, float *_q3);
-    
+
     /**
     @bref   オイラー角で姿勢を取得します.
     @param  val ロール,ピッチ,ヨーの順に配列に格納します.3つ以上の要素の配列を入れてください.
@@ -76,39 +78,63 @@
     Timer madgwickTimer;
     Quaternion q;
     float q0,q1,q2,q3;
-    float beta;  
+    float beta;
+    float zeta;
+    float deltaT_sec;
+    static int counter_numofojbect;
+    int ID;
+
+//  Var for UpadateIMU
+private:
+    float deltaT;
+    unsigned int newTime, oldTime;
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+    float acc_norm;
 };
 
-MadgwickFilter::MadgwickFilter(float B){
+int MadgwickFilter::counter_numofojbect = 0;
+
+MadgwickFilter::MadgwickFilter(float B, float dT_sec)
+{
+    ID = counter_numofojbect;
+    counter_numofojbect++;
     q.w = 1.0f;
     q.x = 0.0f;
     q.y = 0.0f;
     q.z = 0.0f;
     beta = B;
+    zeta = beta * 0.2 /5;
+    deltaT_sec = dT_sec;
     q0 = 1.0f;
     q1 = 0.0f;
     q2 = 0.0f;
-    q3 = 0.0f;   
+    q3 = 0.0f;
     madgwickTimer.start();
 }
 
-void MadgwickFilter::getAttitude(Quaternion *Q){
+void MadgwickFilter::getAttitude(Quaternion *Q)
+{
     *Q = q;
-    return;   
+    return;
 }
 
 
 
-void MadgwickFilter::getAttitude(float *_q0, float *_q1, float *_q2, float *_q3){
+void MadgwickFilter::getAttitude(float *_q0, float *_q1, float *_q2, float *_q3)
+{
     *_q0 = q0;
     *_q1 = q1;
     *_q2 = q2;
     *_q3 = q3;
-    return;   
+    return;
 }
 
 
-void MadgwickFilter::getEulerAngle(float *val){
+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)));
@@ -117,8 +143,193 @@
 //---------------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-inline void MadgwickFilter::MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
-    
+inline void MadgwickFilter::MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
+{
+
+    static float acc_norm;
+    static float deltaT = 0.0f;
+    static unsigned int newTime = 0, oldTime = 0;
+    static float recipNorm;
+    static float s0, s1, s2, s3;
+    static float qDot1, qDot2, qDot3, qDot4;
+    static float w_bx = 0, w_by = 0, w_bz = 0;
+    static float hx=0, hy=0, hz=0;
+    static float init_itr = 0;
+    //static float _2q0mx, _2q0my, _2q0mz, _2q1mx, bx = 0.0f, bz = 1.0f, _2bx, _2bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+    static float _2q0mx, _2q0my, _2q0mz, _2q1mx, bx = 0.0f, bz = 1.0f, _2bx, _2bz, _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)) {
+        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
+        return;
+    }
+    */
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+        // Normalise accelerometer measurement
+        acc_norm = sqrt(ax * ax + ay * ay + az * az);
+        recipNorm = 1.0f / acc_norm;
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;
+
+        // Normalise magnetometer measurement
+        if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+            recipNorm = 1.0f;
+        } else {
+            recipNorm = 1.0f / sqrt(mx * mx + my * my + mz * mz);
+        }
+        mx *= recipNorm;
+        my *= recipNorm;
+        mz *= recipNorm;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0mx = 2.0f * q0 * mx;
+        _2q0my = 2.0f * q0 * my;
+        _2q0mz = 2.0f * q0 * mz;
+        _2q1mx = 2.0f * q1 * mx;
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _2q0q2 = 2.0f * q0 * q2;
+        _2q2q3 = 2.0f * q2 * q3;
+        q0q0 = q0 * q0;
+        q0q1 = q0 * q1;
+        q0q2 = q0 * q2;
+        q0q3 = q0 * q3;
+        q1q1 = q1 * q1;
+        q1q2 = q1 * q2;
+        q1q3 = q1 * q3;
+        q2q2 = q2 * q2;
+        q2q3 = q2 * q3;
+        q3q3 = q3 * q3;
+
+        // Reference direction of Earth's magnetic field
+        //hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+        //hx = mx * (q0q0 + q1q1 - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2);
+        hx = mx
+             - 2.0f * mx * (q2q2 + q3q3)
+             + 2.0f * my * (q1q2 - q0q3)
+             + 2.0f * mz * (q1q3 + q0q2);
+
+        //hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+        //hy = 2.0f * mx * (q0q3 + q1q2) + my * (q0q0 - q1q1 + q2q2 - q3q3) + 2.0f * mz * (q2q3 - q0q1);
+        hy =  2.0f * mx * (q1q2 + q0q3)
+              + my
+              - 2.0f * my * (q1q1 + q3q3)
+              + 2.0f * mz * (q2q3 - q0q1);
+
+        //hz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+        hz =  2.0f * mx * (q1q3 - q0q2)
+              + 2.0f * my * (q2q3 + q0q1)
+              + mz
+              - 2.0f * mz * (q1q1 + q2q2);
+
+
+        bx = sqrt(hx * hx + hy * hy);
+        bz = hz;
+
+        _2bx = 2.0f * bx;
+        _2bz = 2.0f * bz;
+
+        //aux
+        float f_0 = _2q1 * q3 - _2q0 * q2 - ax;
+        float f_1 = _2q0 * q1 + _2q2 * q3 - ay;
+        float f_2 = 1.0f - _2q1 * q1 - _2q2 * q2 - az;
+        float f_3 = _2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx;
+        float f_4 = _2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my;
+        float f_5 = _2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz;
+        float J_00or13 = _2q2; // J_00 negated in matrix multiplication
+        float J_01or12 = 2.0f * q3;
+        float J_02or11 = _2q0; // J_02 negated in matrix multiplication
+        float J_03or10 = _2q1;
+        float J_21 = 2.0f * J_03or10; // negated in matrix multiplication
+        float J_22 = 2.0f * J_00or13; // negated in matrix multiplication
+        float J_30 = _2bz*q2; // negated in matrix multiplication
+        float J_31 = _2bz*q3;
+        float J_32 = 2.0f * _2bx*q2 + _2bz*q0; // negated in matrix multiplication
+        float J_33 = 2.0f * _2bx*q3 - _2bz*q1; // negated in matrix multiplication
+        float J_40 = _2bx*q3 - _2bz*q1; // negated in matrix multiplication
+        float J_41 = _2bx*q2 + _2bz*q0;
+        float J_42 = _2bx*q1 + _2bz*q3;
+        float J_43 = _2bx*q0 - _2bz*q2; // negated in matrix multiplication
+        float J_50 = _2bx*q2;
+        float J_51 = _2bx*q3 - 2.0f * _2bz*q1;
+        float J_52 = _2bx*q0 - 2.0f * _2bz*q2;
+        float J_53 = _2bx*q1;
+
+        // Gradient decent algorithm corrective step
+        /*
+        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) - (_2bx * q3 - _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + bz * q3 * (bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2) - mx) + (bx * q2 + bz * q0) * (bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3) - my) + (bx * q3 - _2bz * q1) * (bx * (q0q2 + q1q3) + bz * (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) + (-_2bx * q2 - bz * q0) * (bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2) - mx) + (bx * q1 + bz * q3) * (bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3) - my) + (bx * q0 - _2bz * q2) * (bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2) - mz);
+        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_2bx * q3 + bz * q1) * (bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2) - mx) + (-bx * q0 + bz * q2) * (bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3) - my) + bx * q1 * (bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2) - mz);
+        */
+        s0 = J_03or10 * f_1 - J_00or13 * f_0 - J_30     * f_3 - J_40 * f_4 + J_50 * f_5;
+        s1 = J_01or12 * f_0 + J_02or11 * f_1 - J_21     * f_2 + J_31 * f_3 + J_41 * f_4 + J_51 * f_5;
+        s2 = J_01or12 * f_1 - J_22     * f_2 - J_02or11 * f_0 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5;
+        s3 = J_03or10 * f_0 + J_00or13 * f_1 - J_33     * f_3 - J_43 * f_4 + J_53 * f_5;
+
+        recipNorm = 1.0f / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+    }
+
+    // Integrate rate of change of quaternion to yield quaternion
+    newTime = (unsigned int)madgwickTimer.read_us();
+    deltaT = (newTime - oldTime) / 1000000.0f;
+    deltaT = fabs(deltaT);
+    oldTime = newTime;
+
+    // compute angular estimated direction of the gyroscope error
+    float w_err_x = _2q0 * s1 - _2q1 * s0 - _2q2 * s3 + _2q3 * s2;
+    float w_err_y = _2q0 * s2 + _2q1 * s3 - _2q2 * s0 - _2q3 * s1;
+    float w_err_z = _2q0 * s3 - _2q1 * s2 + _2q2 * s1 - _2q3 * s0;
+    // compute and remove the gyroscope baises
+    w_bx += w_err_x * deltaT * zeta;
+    w_by += w_err_y * deltaT * zeta;
+    w_bz += w_err_z * deltaT * zeta;
+    gx -= w_bx;
+    gy -= w_by;
+    gz -= w_bz;
+
+
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    q0 += (qDot1 - beta * s0) * deltaT;//(1.0f / sampleFreq);
+    q1 += (qDot2 - beta * s1) * deltaT;//(1.0f / sampleFreq);
+    q2 += (qDot3 - beta * s2) * deltaT;//(1.0f / sampleFreq);
+    q3 += (qDot4 - beta * s3) * deltaT;//(1.0f / sampleFreq);
+
+    // Normalise quaternion
+    recipNorm = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+
+    q.w = q0;
+    q.x = q1;
+    q.y = q2;
+    q.z = q3;
+}
+
+inline void MadgwickFilter::MadgwickAHRSupdate(float *gyro, float *acc, float *mag)
+{
+
+    static float gx = 0, gy = 0, gz = 0.0f, ax = 0.0f, ay = 0.0f, az = 0.0f, mx = 0.0f, my = 0.0f, mz = 0.0f;
     static float acc_norm;
     static float deltaT = 0.0f;
     static unsigned int newTime = 0, oldTime = 0;
@@ -126,7 +337,17 @@
     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;
+    static float _2q0mx, _2q0my, _2q0mz, _2q1mx, bx, bz, _2bx, _2bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+    gx = gyro[0];
+    gy = gyro[1];
+    gz = gyro[2];
+    ax = acc[0];
+    ay = acc[1];
+    az = acc[2];
+    mx = mag[0];
+    my = mag[1];
+    mz = mag[2];
 
     // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
@@ -148,7 +369,7 @@
         recipNorm = 1.0f / acc_norm;
         ax *= recipNorm;
         ay *= recipNorm;
-        az *= recipNorm;   
+        az *= recipNorm;
 
         // Normalise magnetometer measurement
         recipNorm = 1.0f / sqrt(mx * mx + my * my + mz * mz);
@@ -181,16 +402,16 @@
         // Reference direction of Earth's magnetic field
         hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
         hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
-        _2bx = sqrt(hx * hx + hy * hy);
-        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
-        _4bx = 2.0f * _2bx;
-        _4bz = 2.0f * _2bz;
+        bx = sqrt(hx * hx + hy * hy);
+        bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+        _2bx = 2.0f * bx;
+        _2bz = 2.0f * bz;
 
         // Gradient decent algorithm corrective step
-        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-        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);
+        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - bz * q2 * (bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2) - mx) + (-bx * q3 + bz * q1) * (bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3) - my) + bx * q2 * (bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2) - mz);
+        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + bz * q3 * (bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2) - mx) + (bx * q2 + bz * q0) * (bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3) - my) + (bx * q3 - _2bz * q1) * (bx * (q0q2 + q1q3) + bz * (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) + (-_2bx * q2 - bz * q0) * (bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2) - mx) + (bx * q1 + bz * q3) * (bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3) - my) + (bx * q0 - _2bz * q2) * (bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2) - mz);
+        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_2bx * q3 + bz * q1) * (bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2) - mx) + (-bx * q0 + bz * q2) * (bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3) - my) + bx * q1 * (bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2) - mz);
         recipNorm = 1.0f / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
         s0 *= recipNorm;
         s1 *= recipNorm;
@@ -216,7 +437,7 @@
     deltaT = (newTime - oldTime) / 1000000.0f;
     deltaT = fabs(deltaT);
     oldTime = newTime;
-    
+
     q0 += qDot1 * deltaT;//(1.0f / sampleFreq);
     q1 += qDot2 * deltaT;//(1.0f / sampleFreq);
     q2 += qDot3 * deltaT;//(1.0f / sampleFreq);
@@ -228,41 +449,18 @@
     q1 *= recipNorm;
     q2 *= recipNorm;
     q3 *= recipNorm;
-    
+
     q.w = q0;
     q.x = q1;
     q.y = q2;
     q.z = q3;
 }
 
-inline void MadgwickFilter::MadgwickAHRSupdate(float *gyro, float *acc, float *mag){
-    
-    static float gx = 0, gy = 0, gz = 0.0f, ax = 0.0f, ay = 0.0f, az = 0.0f, mx = 0.0f, my = 0.0f, mz = 0.0f;
-    static float acc_norm;
-    static float deltaT = 0.0f;
-    static unsigned int newTime = 0, oldTime = 0;
-    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;
+//---------------------------------------------------------------------------------------------------
+// IMU algorithm update
 
-    gx = gyro[0];
-    gy = gyro[1];
-    gz = gyro[2];
-    ax = acc[0];
-    ay = acc[1];
-    az = acc[2];
-    mx = mag[0];
-    my = mag[1];
-    mz = mag[2];
-
-    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
-    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
-        return;
-    }
-
+inline void MadgwickFilter::MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
+{
     // Rate of change of quaternion from gyroscope
     qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
     qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
@@ -277,120 +475,7 @@
         recipNorm = 1.0f / acc_norm;
         ax *= recipNorm;
         ay *= recipNorm;
-        az *= recipNorm;   
-
-        // Normalise magnetometer measurement
-        recipNorm = 1.0f / sqrt(mx * mx + my * my + mz * mz);
-        mx *= recipNorm;
-        my *= recipNorm;
-        mz *= recipNorm;
-
-        // Auxiliary variables to avoid repeated arithmetic
-        _2q0mx = 2.0f * q0 * mx;
-        _2q0my = 2.0f * q0 * my;
-        _2q0mz = 2.0f * q0 * mz;
-        _2q1mx = 2.0f * q1 * mx;
-        _2q0 = 2.0f * q0;
-        _2q1 = 2.0f * q1;
-        _2q2 = 2.0f * q2;
-        _2q3 = 2.0f * q3;
-        _2q0q2 = 2.0f * q0 * q2;
-        _2q2q3 = 2.0f * q2 * q3;
-        q0q0 = q0 * q0;
-        q0q1 = q0 * q1;
-        q0q2 = q0 * q2;
-        q0q3 = q0 * q3;
-        q1q1 = q1 * q1;
-        q1q2 = q1 * q2;
-        q1q3 = q1 * q3;
-        q2q2 = q2 * q2;
-        q2q3 = q2 * q3;
-        q3q3 = q3 * q3;
-
-        // Reference direction of Earth's magnetic field
-        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
-        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
-        _2bx = sqrt(hx * hx + hy * hy);
-        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
-        _4bx = 2.0f * _2bx;
-        _4bz = 2.0f * _2bz;
-
-        // Gradient decent algorithm corrective step
-        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-        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.0f / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
-        s0 *= recipNorm;
-        s1 *= recipNorm;
-        s2 *= recipNorm;
-        s3 *= recipNorm;
-
-        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.0f*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;
-        qDot3 -= beta * s2;
-        qDot4 -= beta * s3;
-    }
-
-    // Integrate rate of change of quaternion to yield quaternion
-    newTime = (unsigned int)madgwickTimer.read_us();
-    deltaT = (newTime - oldTime) / 1000000.0f;
-    deltaT = fabs(deltaT);
-    oldTime = newTime;
-    
-    q0 += qDot1 * deltaT;//(1.0f / sampleFreq);
-    q1 += qDot2 * deltaT;//(1.0f / sampleFreq);
-    q2 += qDot3 * deltaT;//(1.0f / sampleFreq);
-    q3 += qDot4 * deltaT;//(1.0f / sampleFreq);
-
-    // Normalise quaternion
-    recipNorm = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-    q0 *= recipNorm;
-    q1 *= recipNorm;
-    q2 *= recipNorm;
-    q3 *= recipNorm;
-    
-    q.w = q0;
-    q.x = q1;
-    q.y = q2;
-    q.z = q3; 
-}
-
-//---------------------------------------------------------------------------------------------------
-// IMU algorithm update
-
-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;
-    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);
-    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
-    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
-    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
-
-    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-        // Normalise accelerometer measurement
-        acc_norm = sqrt(ax * ax + ay * ay + az * az);
-        recipNorm = 1.0f / acc_norm;
-        ax *= recipNorm;
-        ay *= recipNorm;
-        az *= recipNorm;   
+        az *= recipNorm;
 
         // Auxiliary variables to avoid repeated arithmetic
         _2q0 = 2.0f * q0;
@@ -408,7 +493,7 @@
         q3q3 = q3 * q3;
 
         // Gradient decent algorithm corrective step
-        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+        s0 = _4q0* q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
         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;
@@ -419,11 +504,11 @@
         s3 *= recipNorm;
 
         // Apply feedback step
-        static float deltaA;
-        deltaA = fabs(acc_norm - 1.00f);
+        //static float deltaA;
+        //deltaA = fabs(acc_norm - 1.00f);
         //beta = 0.5*exp(-20.0*deltaA*deltaA);
-        if(deltaA > 0.3f) beta = 0.0f;
-        else    beta = 0.1f;
+        //if(deltaA > 0.3f) beta = 0.0f;
+        //else    beta = 0.1f;
         qDot1 -= beta * s0;
         qDot2 -= beta * s1;
         qDot3 -= beta * s2;
@@ -431,14 +516,16 @@
     }
 
     // Integrate rate of change of quaternion to yield quaternion
+
     newTime = (unsigned int)madgwickTimer.read_us();
-    deltaT = (newTime - oldTime) / 1000000.0f;
-    deltaT = fabs(deltaT);
+    deltaT = ((newTime - oldTime) % 0xFFFFFFFF) / 1000000.0f;
     oldTime = newTime;
-    q0 += qDot1 * deltaT;;
-    q1 += qDot2 * deltaT;;
-    q2 += qDot3 * deltaT;;
-    q3 += qDot4 * deltaT;;
+    deltaT = fabs(deltaT);
+
+    q0 += qDot1 * deltaT;
+    q1 += qDot2 * deltaT;
+    q2 += qDot3 * deltaT;
+    q3 += qDot4 * deltaT;
 
     // Normalise quaternion
     recipNorm = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);