Dynamic Sfusion

.github/workflows/actions.yml

@@ -15,7 +15,7 @@ jobs:
       - uses: actions/checkout@v4
       - uses: jidicula/[email protected]
         with:
-          clang-format-version: "17"
+          clang-format-version: "18"
           fallback-style: google
 #      Disable clang-tidy for now
 #      - name: Get clang-tidy

lib/vqf/vqf.cpp

@@ -18,41 +18,6 @@
 
 inline vqf_real_t square(vqf_real_t x) { return x*x; }
 
-
-VQFParams::VQFParams()
-    : tauAcc(3.0)
-    , tauMag(9.0)
-#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-    , motionBiasEstEnabled(true)
-#endif
-    , restBiasEstEnabled(true)
-    , magDistRejectionEnabled(true)
-    , biasSigmaInit(0.5)
-    , biasForgettingTime(100.0)
-    , biasClip(2.0)
-#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-    , biasSigmaMotion(0.1)
-    , biasVerticalForgettingFactor(0.0001)
-#endif
-    , biasSigmaRest(0.03)
-    , restMinT(1.5)
-    , restFilterTau(0.5)
-    , restThGyr(2.0)
-    , restThAcc(0.5)
-    , magCurrentTau(0.05)
-    , magRefTau(20.0)
-    , magNormTh(0.1)
-    , magDipTh(10.0)
-    , magNewTime(20.0)
-    , magNewFirstTime(5.0)
-    , magNewMinGyr(20.0)
-    , magMinUndisturbedTime(0.5)
-    , magMaxRejectionTime(60.0)
-    , magRejectionFactor(2.0)
-{
-
-}
-
 VQF::VQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs)
 {
     coeffs.gyrTs = gyrTs;
@@ -917,16 +882,8 @@ void VQF::setup()
 
     coeffs.biasP0 = square(params.biasSigmaInit*100.0);
     // the system noise increases the variance from 0 to (0.1 °/s)^2 in biasForgettingTime seconds
-    coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime;
-
-#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
-    vqf_real_t pMotion = square(params.biasSigmaMotion*100.0);
-    coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion;
-    coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10));
-#endif
 
-    vqf_real_t pRest = square(params.biasSigmaRest*100.0);
-    coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
+	updateBiasForgettingTime(params.biasForgettingTime);
 
     filterCoeffs(params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA);
     filterCoeffs(params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA);
@@ -941,3 +898,16 @@ void VQF::setup()
 
     resetState();
 }
+
+void VQF::updateBiasForgettingTime(float biasForgettingTime) {
+    coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime;
+
+#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
+    vqf_real_t pMotion = square(params.biasSigmaMotion*100.0);
+    coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion;
+    coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10));
+#endif
+
+    vqf_real_t pRest = square(params.biasSigmaRest*100.0);
+    coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
+}

lib/vqf/vqf.h

@@ -11,7 +11,7 @@
 #include <stddef.h>
 
 #define VQF_SINGLE_PRECISION
-#define VQF_NO_MOTION_BIAS_ESTIMATION
+// #define VQF_NO_MOTION_BIAS_ESTIMATION
 #define M_PI 3.14159265358979323846
 #define M_SQRT2 1.41421356237309504880
 
@@ -41,11 +41,6 @@ typedef float vqf_real_t;
  */
 struct VQFParams
 {
-    /**
-     * @brief Constructor that initializes the struct with the default parameters.
-     */
-    VQFParams();
-
     /**
      * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering in seconds.
      *
@@ -57,7 +52,7 @@ struct VQFParams
      *
      * Default value: 3.0 s
      */
-    vqf_real_t tauAcc;
+    vqf_real_t tauAcc = 3.0;
     /**
      * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds.
      *
@@ -70,7 +65,7 @@ struct VQFParams
      *
      * Default value: 9.0 s
      */
-    vqf_real_t tauMag;
+    vqf_real_t tauMag = 9.0;
 
 #ifndef VQF_NO_MOTION_BIAS_ESTIMATION
     /**
@@ -79,15 +74,15 @@ struct VQFParams
      * If set to true (default), gyroscope bias is estimated based on the inclination correction only, i.e. without
      * using magnetometer measurements.
      */
-    bool motionBiasEstEnabled;
+    bool motionBiasEstEnabled = true;
 #endif
     /**
      * @brief Enables rest detection and gyroscope bias estimation during rest phases.
      *
      * If set to true (default), phases in which the IMU is at rest are detected. During rest, the gyroscope bias
      * is estimated from the low-pass filtered gyroscope readings.
      */
-    bool restBiasEstEnabled;
+    bool restBiasEstEnabled = true;
     /**
      * @brief Enables magnetic disturbance detection and magnetic disturbance rejection.
      *
@@ -96,22 +91,22 @@ struct VQFParams
      * the disturbances exceeds #magMaxRejectionTime, magnetometer-based updates are performed, but with an increased
      * time constant.
      */
-    bool magDistRejectionEnabled;
+    bool magDistRejectionEnabled = true;
 
     /**
      * @brief Standard deviation of the initial bias estimation uncertainty (in degrees per second).
      *
      * Default value: 0.5 °/s
      */
-    vqf_real_t biasSigmaInit;
+    vqf_real_t biasSigmaInit = 0.5;
     /**
      * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 °/s (in seconds).
      *
      * This value determines the system noise assumed by the Kalman filter.
      *
      * Default value: 100.0 s
      */
-    vqf_real_t biasForgettingTime;
+    vqf_real_t biasForgettingTime = 100.0;
     /**
      * @brief Maximum expected gyroscope bias (in degrees per second).
      *
@@ -121,7 +116,7 @@ struct VQFParams
      *
      * Default value: 2.0 °/s
      */
-    vqf_real_t biasClip;
+    vqf_real_t biasClip = 2.0;
 #ifndef VQF_NO_MOTION_BIAS_ESTIMATION
     /**
      * @brief Standard deviation of the converged bias estimation uncertainty during motion (in degrees per second).
@@ -130,7 +125,7 @@ struct VQFParams
      *
      * Default value: 0.1 °/s
      */
-    vqf_real_t biasSigmaMotion;
+    vqf_real_t biasSigmaMotion = 0.1125;
     /**
      * @brief Forgetting factor for unobservable bias in vertical direction during motion.
      *
@@ -140,7 +135,7 @@ struct VQFParams
      *
      * Default value: 0.0001
      */
-    vqf_real_t biasVerticalForgettingFactor;
+    vqf_real_t biasVerticalForgettingFactor = 0.0001;
 #endif
     /**
      * @brief Standard deviation of the converged bias estimation uncertainty during rest (in degrees per second).
@@ -149,7 +144,7 @@ struct VQFParams
      *
      * Default value: 0.03 °
      */
-    vqf_real_t biasSigmaRest;
+    vqf_real_t biasSigmaRest = 0.03;
 
     /**
      * @brief Time threshold for rest detection (in seconds).
@@ -158,7 +153,7 @@ struct VQFParams
      *
      * Default value: 1.5 s
      */
-    vqf_real_t restMinT;
+    vqf_real_t restMinT = 0.5;
     /**
      * @brief Time constant for the low-pass filter used in rest detection (in seconds).
      *
@@ -167,7 +162,7 @@ struct VQFParams
      *
      * Default value: 0.5 s
      */
-    vqf_real_t restFilterTau;
+    vqf_real_t restFilterTau = 0.5;
     /**
      * @brief Angular velocity threshold for rest detection (in °/s).
      *
@@ -176,7 +171,7 @@ struct VQFParams
      *
      * Default value: 2.0 °/s
      */
-    vqf_real_t restThGyr;
+    vqf_real_t restThGyr = 2.0;
     /**
      * @brief Acceleration threshold for rest detection (in m/s²).
      *
@@ -185,7 +180,7 @@ struct VQFParams
      *
      * Default value: 0.5 m/s²
      */
-    vqf_real_t restThAcc;
+    vqf_real_t restThAcc = 0.5;
 
     /**
      * @brief Time constant for current norm/dip value in magnetic disturbance detection (in seconds).
@@ -196,29 +191,29 @@ struct VQFParams
      *
      * Default value: 0.05 s
      */
-    vqf_real_t magCurrentTau;
+    vqf_real_t magCurrentTau = 0.05;
     /**
      * @brief Time constant for the adjustment of the magnetic field reference (in seconds).
      *
      * This adjustment allows the reference estimate to converge to the observed undisturbed field.
      *
      * Default value: 20.0 s
      */
-    vqf_real_t magRefTau;
+    vqf_real_t magRefTau = 20.0;
     /**
      * @brief Relative threshold for the magnetic field strength for magnetic disturbance detection.
      *
      * This value is relative to the reference norm.
      *
      * Default value: 0.1 (10%)
      */
-    vqf_real_t magNormTh;
+    vqf_real_t magNormTh = 0.1;
     /**
      * @brief Threshold for the magnetic field dip angle for magnetic disturbance detection (in degrees).
      *
      * Default vaule: 10 °
      */
-    vqf_real_t magDipTh;
+    vqf_real_t magDipTh = 10.0;
     /**
      * @brief Duration after which to accept a different homogeneous magnetic field (in seconds).
      *
@@ -228,7 +223,7 @@ struct VQFParams
      *
      * Default value: 20.0
      */
-    vqf_real_t magNewTime;
+    vqf_real_t magNewTime = 20.0;
     /**
      * @brief Duration after which to accept a homogeneous magnetic field for the first time (in seconds).
      *
@@ -237,21 +232,21 @@ struct VQFParams
      *
      * Default value: 5.0
      */
-    vqf_real_t magNewFirstTime;
+    vqf_real_t magNewFirstTime = 5.0;
     /**
      * @brief Minimum angular velocity needed in order to count time for new magnetic field acceptance (in °/s).
      *
      * Durations for which the angular velocity norm is below this threshold do not count towards reaching #magNewTime.
      *
      * Default value: 20.0 °/s
      */
-    vqf_real_t magNewMinGyr;
+    vqf_real_t magNewMinGyr = 20.0;
     /**
      * @brief Minimum duration within thresholds after which to regard the field as undisturbed again (in seconds).
      *
      * Default value: 0.5 s
      */
-    vqf_real_t magMinUndisturbedTime;
+    vqf_real_t magMinUndisturbedTime = 0.5;
     /**
      * @brief Maximum duration of full magnetic disturbance rejection (in seconds).
      *
@@ -262,7 +257,7 @@ struct VQFParams
      *
      * Default value: 60.0 s
      */
-    vqf_real_t magMaxRejectionTime;
+    vqf_real_t magMaxRejectionTime = 60.0;
     /**
      * @brief Factor by which to slow the heading correction during long disturbed phases.
      *
@@ -274,7 +269,7 @@ struct VQFParams
      *
      * Default value: 2.0
      */
-    vqf_real_t magRejectionFactor;
+    vqf_real_t magRejectionFactor = 2.0;
 };
 
 /**
@@ -987,6 +982,8 @@ class VQF
     static bool matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]);
 #endif
 
+	void updateBiasForgettingTime(float biasForgettingTime);
+
 protected:
     /**
      * @brief Calculates coefficients based on parameters and sampling rates.

platformio-tools.ini

@@ -23,18 +23,22 @@ board = esp12e
 [env:BOARD_NODEMCU]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+board_build.f_cpu = 160000000L
 
 [env:BOARD_WEMOSD1MINI]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+board_build.f_cpu = 160000000L
 
 [env:BOARD_TTGO_TBASE]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+board_build.f_cpu = 160000000L
 
 [env:BOARD_WEMOSWROOM02]
 platform = espressif8266 @ 4.2.1
 board = esp12e
+board_build.f_cpu = 160000000L
 
 [env:BOARD_WROOM32]
 platform = espressif32 @ 6.7.0