From d3849c0265eb5072c5a08e00d197b9f758f3650b Mon Sep 17 00:00:00 2001
From: Thomas Stastny <thomas.stastny@auterion.com>
Date: Wed, 28 Sep 2022 17:47:35 +0200
Subject: [PATCH] welford mean: convert to matrix only template

---
 src/lib/mathlib/math/WelfordMean.hpp             | 16 ++++++++--------
 src/lib/mathlib/math/WelfordMeanTest.cpp         |  2 +-
 src/modules/gyro_calibration/GyroCalibration.hpp |  2 +-
 src/modules/sensors/vehicle_imu/VehicleIMU.hpp   | 10 +++++-----
 4 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/src/lib/mathlib/math/WelfordMean.hpp b/src/lib/mathlib/math/WelfordMean.hpp
index e5b0240533..ac6af40faa 100644
--- a/src/lib/mathlib/math/WelfordMean.hpp
+++ b/src/lib/mathlib/math/WelfordMean.hpp
@@ -42,17 +42,17 @@
 namespace math
 {
 
-template<typename T>
+template <typename Type, size_t N>
 class WelfordMean
 {
 public:
 	// For a new value, compute the new count, new mean, the new M2.
-	void update(const T &new_value)
+	void update(const matrix::Vector<Type, N> &new_value)
 	{
 		_count++;
 
 		// mean accumulates the mean of the entire dataset
-		const T delta{new_value - _mean};
+		const matrix::Vector<Type, N> delta{new_value - _mean};
 		_mean += delta / _count;
 
 		// M2 aggregates the squared distance from the mean
@@ -71,12 +71,12 @@ public:
 	}
 
 	// Retrieve the mean, variance and sample variance
-	T mean() const { return _mean; }
-	T variance() const { return _M2 / _count; }
-	T sample_variance() const { return _M2 / (_count - 1); }
+	matrix::Vector<Type, N> mean() const { return _mean; }
+	matrix::Vector<Type, N> variance() const { return _M2 / _count; }
+	matrix::Vector<Type, N> sample_variance() const { return _M2 / (_count - 1); }
 private:
-	T _mean{};
-	T _M2{};
+	matrix::Vector<Type, N> _mean{};
+	matrix::Vector<Type, N> _M2{};
 	unsigned _count{0};
 };
 
diff --git a/src/lib/mathlib/math/WelfordMeanTest.cpp b/src/lib/mathlib/math/WelfordMeanTest.cpp
index 06b59e6dfb..15abbe10c4 100644
--- a/src/lib/mathlib/math/WelfordMeanTest.cpp
+++ b/src/lib/mathlib/math/WelfordMeanTest.cpp
@@ -45,7 +45,7 @@ TEST(WelfordMeanTest, NoisySignal)
 	std::normal_distribution<float> standard_normal_distribution{0.f, std_dev};
 	std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed
 	random_generator.seed(42);
-	WelfordMean<Vector3f> welford{};
+	WelfordMean<float, 3> welford{};
 
 	for (int i = 0; i < 50; i++) {
 		const float noisy_value = standard_normal_distribution(random_generator);
diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp
index c12d24f8bc..a16e83c4a2 100644
--- a/src/modules/gyro_calibration/GyroCalibration.hpp
+++ b/src/modules/gyro_calibration/GyroCalibration.hpp
@@ -93,7 +93,7 @@ private:
 	uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS>  _sensor_gyro_subs{ORB_ID::sensor_gyro};
 
 	calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
-	math::WelfordMean<matrix::Vector3f> _gyro_mean[MAX_SENSORS] {};
+	math::WelfordMean<float, 3> _gyro_mean[MAX_SENSORS] {};
 	float _temperature[MAX_SENSORS] {};
 	hrt_abstime _gyro_last_update[MAX_SENSORS] {};
 
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
index 710d17fe97..130efc390f 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
@@ -119,13 +119,13 @@ private:
 	hrt_abstime _gyro_timestamp_sample_last{0};
 	hrt_abstime _gyro_timestamp_last{0};
 
-	math::WelfordMean<matrix::Vector3f> _raw_accel_mean{};
-	math::WelfordMean<matrix::Vector3f> _raw_gyro_mean{};
+	math::WelfordMean<float, 3> _raw_accel_mean{};
+	math::WelfordMean<float, 3> _raw_gyro_mean{};
 
-	math::WelfordMean<matrix::Vector2f> _accel_interval_mean{};
-	math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
+	math::WelfordMean<float, 2> _accel_interval_mean{};
+	math::WelfordMean<float, 2> _gyro_interval_mean{};
 
-	math::WelfordMean<matrix::Vector2f> _gyro_update_latency_mean{};
+	math::WelfordMean<float, 2> _gyro_update_latency_mean{};
 
 	float _accel_interval_best_variance{(float)INFINITY};
 	float _gyro_interval_best_variance{(float)INFINITY};