PQ_Hybrid_Electrical_Equipment_Team / MadgwickFilter

Madgwick Filterをライブラリ化しました．内容はオープンソースになっていたやつのほぼ同じです．

Dependencies:   Quaternion

Dependents:   Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET Hybrid_IZU2019 ... more

Fork of MadgwickFilter by Gaku Matsumoto

Diff: MadgwickFilter.hpp

Revision:
3:715924156fcb
Parent:
2:e1de76e257f6
--- a/MadgwickFilter.hpp	Sat Jan 28 21:04:15 2017 +0000
+++ b/MadgwickFilter.hpp	Mon Jul 17 10:05:45 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 = 0.3*exp(-30.0*deltaA*deltaA);
+        //printf("%f\r\n", beta);
+        //beta = 0.1;
+        //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;