Change relevant doubles to floats

lib/vqf/vqf.cpp

@@ -2,7 +2,7 @@
 //
 // SPDX-License-Identifier: MIT
 
-// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs)
+// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
 // Removed batch update functions
 
 #include "vqf.h"
@@ -38,7 +38,7 @@ VQF::VQF(const VQFParams &params, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t
     setup();
 }
 
-void VQF::updateGyr(const vqf_real_t gyr[3], double gyrTs)
+void VQF::updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
 {
     // rest detection
     if (params.restBiasEstEnabled || params.magDistRejectionEnabled) {
@@ -502,8 +502,8 @@ void VQF::setTauAcc(vqf_real_t tauAcc)
         return;
     }
     params.tauAcc = tauAcc;
-    double newB[3];
-    double newA[3];
+    vqf_real_t newB[3];
+    vqf_real_t newA[3];
 
     filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA);
     filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState);
@@ -696,15 +696,15 @@ vqf_real_t VQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts)
     }
 }
 
-void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA[])
+void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[], vqf_real_t outA[])
 {
     assert(tau > 0);
     assert(Ts > 0);
     // second order Butterworth filter based on https://stackoverflow.com/a/52764064
-    double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response
-    double C = tan(M_PI*fc*double(Ts));
-    double D = C*C + sqrt(2)*C + 1;
-    double b0 = C*C/D;
+    vqf_real_t fc = (M_SQRT2 / (2.0*M_PI))/vqf_real_t(tau); // time constant of dampened, non-oscillating part of step response
+    vqf_real_t C = tan(M_PI*fc*vqf_real_t(Ts));
+    vqf_real_t D = C*C + sqrt(2)*C + 1;
+    vqf_real_t b0 = C*C/D;
     outB[0] = b0;
     outB[1] = 2*b0;
     outB[2] = b0;
@@ -713,17 +713,17 @@ void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA
     outA[1] = (1-sqrt(2)*C+C*C)/D; // a2
 }
 
-void VQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[])
+void VQF::filterInitialState(vqf_real_t x0, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t out[])
 {
     // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter
     // update equation)
     out[0] = x0*(1 - b[0]);
     out[1] = x0*(b[2] - a[1]);
 }
 
-void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[],
-                                         const double a_old[], const double b_new[],
-                                         const double a_new[], double state[])
+void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const vqf_real_t b_old[],
+                                         const vqf_real_t a_old[], const vqf_real_t b_new[],
+                                         const vqf_real_t a_new[], vqf_real_t state[])
 {
     if (isnan(state[0])) {
         return;
@@ -734,18 +734,18 @@ void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const do
     }
 }
 
-vqf_real_t VQF::filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2])
+vqf_real_t VQF::filterStep(vqf_real_t x, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t state[2])
 {
     // difference equations based on scipy.signal.lfilter documentation
     // assumes that a0 == 1.0
-    double y = b[0]*x + state[0];
+    vqf_real_t y = b[0]*x + state[0];
     state[0] = b[1]*x - a[0]*y + state[1];
     state[1] = b[2]*x - a[1]*y;
     return y;
 }
 
-void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3],
-                    const double a[2], double state[], vqf_real_t out[])
+void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const vqf_real_t b[3],
+                    const vqf_real_t a[2], vqf_real_t state[], vqf_real_t out[])
 {
     assert(N>=2);
 
@@ -838,17 +838,17 @@ void VQF::matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2
 bool VQF::matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9])
 {
     // in = [a b c; d e f; g h i]
-    double A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
-    double D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
-    double G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
-    double B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
-    double E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
-    double H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
-    double C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
-    double F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
-    double I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
-
-    double det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
+    vqf_real_t A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
+    vqf_real_t D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
+    vqf_real_t G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
+    vqf_real_t B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
+    vqf_real_t E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
+    vqf_real_t H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
+    vqf_real_t C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
+    vqf_real_t F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
+    vqf_real_t I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
+
+    vqf_real_t det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
 
     if (det >= -EPS && det <= EPS) {
         std::fill(out, out+9, 0);
@@ -909,4 +909,4 @@ void VQF::updateBiasForgettingTime(float biasForgettingTime) {
 
     vqf_real_t pRest = square(params.biasSigmaRest*100.0);
     coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
-}
+}

lib/vqf/vqf.h

@@ -2,7 +2,7 @@
 //
 // SPDX-License-Identifier: MIT
 
-// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs)
+// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
 // Removed batch update functions
 
 #ifndef VQF_HPP
@@ -17,9 +17,9 @@
 /**
  * @brief Typedef for the floating-point data type used for most operations.
  *
- * By default, all floating-point calculations are performed using `double`. Set the
+ * By default, all floating-point calculations are performed using `vqf_real_t`. Set the
  * `VQF_SINGLE_PRECISION` define to change this type to `float`. Note that the
- * Butterworth filter implementation will always use double precision as using floats
+ * Butterworth filter implementation will always use vqf_real_t precision as using floats
  * can cause numeric issues.
  */
 #ifndef VQF_SINGLE_PRECISION
@@ -355,7 +355,7 @@ struct VQFState {
 	/**
 	 * @brief Internal low-pass filter state for #lastAccLp.
 	 */
-	double accLpState[3 * 2];
+	vqf_real_t accLpState[3 * 2];
 	/**
 	 * @brief Last inclination correction angular rate.
 	 *
@@ -413,12 +413,12 @@ struct VQFState {
 	 * @brief Internal state of the Butterworth low-pass filter for the rotation matrix
 	 * coefficients used in motion bias estimation.
 	 */
-	double motionBiasEstRLpState[9 * 2];
+	vqf_real_t motionBiasEstRLpState[9 * 2];
 	/**
 	 * @brief Internal low-pass filter state for the rotated bias estimate used in
 	 * motion bias estimation.
 	 */
-	double motionBiasEstBiasLpState[2 * 2];
+	vqf_real_t motionBiasEstBiasLpState[2 * 2];
 #endif
 	/**
 	 * @brief Last (squared) deviations from the reference of the last sample used in
@@ -451,7 +451,7 @@ struct VQFState {
 	/**
 	 * @brief Internal low-pass filter state for #restLastGyrLp.
 	 */
-	double restGyrLpState[3 * 2];
+	vqf_real_t restGyrLpState[3 * 2];
 	/**
 	 * @brief Last low-pass filtered accelerometer measurement used as the reference for
 	 * rest detection.
@@ -460,7 +460,7 @@ struct VQFState {
 	/**
 	 * @brief Internal low-pass filter state for #restLastAccLp.
 	 */
-	double restAccLpState[3 * 2];
+	vqf_real_t restAccLpState[3 * 2];
 
 	/**
 	 * @brief Norm of the currently accepted magnetic field reference.
@@ -514,7 +514,7 @@ struct VQFState {
 	/**
 	 * @brief Internal low-pass filter state for the current norm and dip angle.
 	 */
-	double magNormDipLpState[2 * 2];
+	vqf_real_t magNormDipLpState[2 * 2];
 };
 
 /**
@@ -542,13 +542,13 @@ struct VQFCoefficients {
 	 *
 	 * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$.
 	 */
-	double accLpB[3];
+	vqf_real_t accLpB[3];
 	/**
 	 * @brief Denominator coefficients of the acceleration low-pass filter.
 	 *
 	 * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$.
 	 */
-	double accLpA[2];
+	vqf_real_t accLpA[2];
 
 	/**
 	 * @brief Gain of the first-order filter used for heading correction.
@@ -584,22 +584,22 @@ struct VQFCoefficients {
 	 * @brief Numerator coefficients of the gyroscope measurement low-pass filter for
 	 * rest detection.
 	 */
-	double restGyrLpB[3];
+	vqf_real_t restGyrLpB[3];
 	/**
 	 * @brief Denominator coefficients of the gyroscope measurement low-pass filter for
 	 * rest detection.
 	 */
-	double restGyrLpA[2];
+	vqf_real_t restGyrLpA[2];
 	/**
 	 * @brief Numerator coefficients of the accelerometer measurement low-pass filter
 	 * for rest detection.
 	 */
-	double restAccLpB[3];
+	vqf_real_t restAccLpB[3];
 	/**
 	 * @brief Denominator coefficients of the accelerometer measurement low-pass filter
 	 * for rest detection.
 	 */
-	double restAccLpA[2];
+	vqf_real_t restAccLpA[2];
 
 	/**
 	 * @brief Gain of the first-order filter used for to update the magnetic field
@@ -610,12 +610,12 @@ struct VQFCoefficients {
 	 * @brief Numerator coefficients of the low-pass filter for the current magnetic
 	 * norm and dip.
 	 */
-	double magNormDipLpB[3];
+	vqf_real_t magNormDipLpB[3];
 	/**
 	 * @brief Denominator coefficients of the low-pass filter for the current magnetic
 	 * norm and dip.
 	 */
-	double magNormDipLpA[2];
+	vqf_real_t magNormDipLpA[2];
 };
 
 /**
@@ -723,7 +723,7 @@ class VQF {
 	 *
 	 * @param gyr gyroscope measurement in rad/s
 	 */
-	void updateGyr(const vqf_real_t gyr[3], double gyrTs);
+	void updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs);
 	/**
 	 * @brief Performs accelerometer update step.
 	 *
@@ -979,7 +979,7 @@ class VQF {
 	 * @param outA output array for denominator coefficients (without \f$a_0=1\f$)
 	 */
 	static void
-	filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]);
+	filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[3], vqf_real_t outA[2]);
 	/**
 	 * @brief Calculates the initial filter state for a given steady-state value.
 	 * @param x0 steady state value
@@ -989,9 +989,9 @@ class VQF {
 	 */
 	static void filterInitialState(
 		vqf_real_t x0,
-		const double b[],
-		const double a[],
-		double out[2]
+		const vqf_real_t b[],
+		const vqf_real_t a[],
+		vqf_real_t out[2]
 	);
 	/**
 	 * @brief Adjusts the filter state when changing coefficients.
@@ -1012,11 +1012,11 @@ class VQF {
 	static void filterAdaptStateForCoeffChange(
 		vqf_real_t last_y[],
 		size_t N,
-		const double b_old[3],
-		const double a_old[2],
-		const double b_new[3],
-		const double a_new[2],
-		double state[]
+		const vqf_real_t b_old[3],
+		const vqf_real_t a_old[2],
+		const vqf_real_t b_new[3],
+		const vqf_real_t a_new[2],
+		vqf_real_t state[]
 	);
 	/**
 	 * @brief Performs a filter step for a scalar value.
@@ -1027,7 +1027,7 @@ class VQF {
 	 * @return filtered value
 	 */
 	static vqf_real_t
-	filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]);
+	filterStep(vqf_real_t x, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t state[2]);
 	/**
 	 * @brief Performs filter step for vector-valued signal with averaging-based
 	 * initialization.
@@ -1052,9 +1052,9 @@ class VQF {
 		size_t N,
 		vqf_real_t tau,
 		vqf_real_t Ts,
-		const double b[3],
-		const double a[2],
-		double state[],
+		const vqf_real_t b[3],
+		const vqf_real_t a[2],
+		vqf_real_t state[],
 		vqf_real_t out[]
 	);
 #ifndef VQF_NO_MOTION_BIAS_ESTIMATION

src/sensors/softfusion/CalibrationBase.h

@@ -42,8 +42,8 @@ class CalibrationBase {
 		uint8_t sensorId,
 		SlimeVR::Logging::Logger& logger,
 		float TempTs,
-		double AScale,
-		double GScale
+		float AScale,
+		float GScale
 	)
 		: fusion{fusion}
 		, sensor{sensor}
@@ -115,8 +115,8 @@ class CalibrationBase {
 	uint8_t sensorId;
 	SlimeVR::Logging::Logger& logger;
 	float TempTs;
-	double AScale;
-	double GScale;
+	float AScale;
+	float GScale;
 };
 
 }  // namespace SlimeVR::Sensor

src/sensors/softfusion/SoftfusionCalibration.h

@@ -49,8 +49,8 @@ class SoftfusionCalibrator : public CalibrationBase<IMU, RawSensorT, RawVectorT>
 		uint8_t sensorId,
 		SlimeVR::Logging::Logger& logger,
 		float TempTs,
-		double AScale,
-		double GScale
+		float AScale,
+		float GScale
 	)
 		: Base{fusion, sensor, sensorId, logger, TempTs, AScale, GScale} {
 		calibration.T_Ts = TempTs;

src/sensors/softfusion/nonblockingcalibration/NonBlockingCalibration.h

@@ -55,8 +55,8 @@ class NonBlockingCalibrator
 		uint8_t sensorId,
 		Logging::Logger& logger,
 		float TempTs,
-		double AScale,
-		double GScale
+		float AScale,
+		float GScale
 	)
 		: Base(fusion, imu, sensorId, logger, TempTs, AScale, GScale) {
 		calibration.T_Ts = TempTs;

src/sensors/softfusion/softfusionsensor.h

@@ -61,9 +61,9 @@ class SoftFusionSensor : public Sensor {
 		typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
 	using RawVectorT = std::array<RawSensorT, 3>;
 
-	static constexpr double GScale
+	static constexpr float GScale
 		= ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0);
-	static constexpr double AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity;
+	static constexpr float AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity;
 
 	using Calib = Calibrator<imu, RawSensorT, RawVectorT>;