welford mean: convert to matrix only template

PX4 · Oct 5, 2022 · fbef296 · fbef296
1 parent 6ce38b8
commit fbef296
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 15 deletions.
diff --git 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 @@ class WelfordMean
 	}
 
 	// 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
@@ -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
@@ -93,7 +93,7 @@ class GyroCalibration : public ModuleBase<GyroCalibration>, public ModuleParams,
 	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
@@ -119,13 +119,13 @@ class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem
 	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};