create Welford mean Vector with covariance and improve precision with Kahan summation (#20676)

- WelfordMeanVector now computes covariance - use Kahan summation for Welford mean (but continue using float32 for actual mean, etc) - WelfordMean and WelfordMeanVector handle initial value and count roll over - Welford mean count rollover at 16 bit max to prevent numerical issues and shift weight to newer samples - sensors/vehicle_imu: update Welford mean usage (now simplified with resets removed) - fix vehicle_imu_status accel var, now properly rotated with full covariance matrix - gyro_calibration: update Welford mean usage
2026-06-30 02:50:34 +08:00 · 2022-11-30 14:50:13 -05:00
parent 5155346d60
commit 89b81b0bd6
9 changed files with 527 additions and 248 deletions
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -39,48 +39,97 @@

 #pragma once

+#include <lib/mathlib/mathlib.h>
+
 namespace math
 {

-template <typename Type, size_t N>
+template <typename Type = float>
 class WelfordMean
 {
 public:
 	// For a new value, compute the new count, new mean, the new M2.
-	void update(const matrix::Vector<Type, N> &new_value)
+	bool update(const Type &new_value)
 	{
-		_count++;
+		if (_count == 0) {
+			reset();
+			_count = 1;
+			_mean = new_value;
+			return false;
+
+		} else if (_count == UINT16_MAX) {
+			// count overflow
+			//  reset count, but maintain mean and variance
+			_M2 = _M2 / _count;
+			_M2_accum = 0;
+
+			_count = 1;
+
+		} else {
+			_count++;
+		}

 		// mean accumulates the mean of the entire dataset
-		const matrix::Vector<Type, N> delta{new_value - _mean};
-		_mean += delta / _count;
+		// delta can be very small compared to the mean, use algorithm to minimise numerical error
+		const Type delta{new_value - _mean};
+		const Type mean_change = delta / _count;
+		_mean = kahanSummation(_mean, mean_change, _mean_accum);

 		// M2 aggregates the squared distance from the mean
 		// count aggregates the number of samples seen so far
-		_M2 += delta.emult(new_value - _mean);
+		const Type M2_change = delta * (new_value - _mean);
+		_M2 = kahanSummation(_M2, M2_change, _M2_accum);

 		// protect against floating point precision causing negative variances
-		_M2 = matrix::max(_M2, {});
+		_M2 = math::max(_M2, 0.f);
+
+		if (!PX4_ISFINITE(_mean) || !PX4_ISFINITE(_M2)) {
+			reset();
+			return false;
+		}
+
+		return valid();
 	}

 	bool valid() const { return _count > 2; }
-	unsigned count() const { return _count; }
+	auto count() const { return _count; }

 	void reset()
 	{
 		_count = 0;
-		_mean = {};
-		_M2 = {};
+		_mean = 0;
+		_M2 = 0;
+
+		_mean_accum = 0;
+		_M2_accum = 0;
 	}

 	// Retrieve the mean, variance and sample variance
-	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); }
+	Type mean() const { return _mean; }
+	Type variance() const { return _M2 / (_count - 1); }
+	Type standard_deviation() const { return std::sqrt(variance()); }
+
 private:
-	matrix::Vector<Type, N> _mean{};
-	matrix::Vector<Type, N> _M2{};
-	unsigned _count{0};
+
+	// Use Kahan summation algorithm to get the sum of "sum_previous" and "input".
+	// This function relies on the caller to be responsible for keeping a copy of
+	// "accumulator" and passing this value at the next iteration.
+	// Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm
+	inline Type kahanSummation(Type sum_previous, Type input, Type &accumulator)
+	{
+		const Type y = input - accumulator;
+		const Type t = sum_previous + y;
+		accumulator = (t - sum_previous) - y;
+		return t;
+	}
+
+	Type _mean{};
+	Type _M2{};
+
+	Type _mean_accum{};  ///< kahan summation algorithm accumulator for mean
+	Type _M2_accum{};    ///< kahan summation algorithm accumulator for M2
+
+	uint16_t _count{0};
 };

 } // namespace math
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2021-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -45,22 +45,20 @@ 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<float, 3> welford{};
+	WelfordMean<float> welford{};

-	for (int i = 0; i < 50; i++) {
+	for (int i = 0; i < 1000; i++) {
 		const float noisy_value = standard_normal_distribution(random_generator);
-		welford.update(Vector3f(noisy_value, noisy_value - 1.f, noisy_value + 1.f));
+		welford.update(noisy_value);
 	}

 	EXPECT_TRUE(welford.valid());
-	const Vector3f mean = welford.mean();
-	const Vector3f var = welford.variance();
+	const float mean = welford.mean();
+	const float var = welford.variance();
 	const float var_real = std_dev * std_dev;

-	EXPECT_NEAR(mean(0), 0.f, 0.7f);
-	EXPECT_NEAR(mean(1), -1.f, 0.7f);
-	EXPECT_NEAR(mean(2), 1.f, 0.7f);
-	EXPECT_NEAR(var(0), var_real, 0.1f);
-	EXPECT_NEAR(var(1), var_real, 0.1f);
-	EXPECT_NEAR(var(2), var_real, 0.1f);
+	EXPECT_NEAR(mean, 0.f, 0.7f);
+
+	EXPECT_NEAR(var, var_real, 0.1f);
+
 }
@@ -0,0 +1,161 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file WelfordMeanVector.hpp
+ *
+ * Welford's online algorithm for computing mean and covariance of a vector.
+ */
+
+#pragma once
+
+namespace math
+{
+
+template <typename Type, size_t N>
+class WelfordMeanVector
+{
+public:
+	// For a new value, compute the new count, new mean, the new M2.
+	bool update(const matrix::Vector<Type, N> &new_value)
+	{
+		if (_count == 0) {
+			reset();
+			_count = 1;
+			_mean = new_value;
+			return false;
+
+		} else if (_count == UINT16_MAX) {
+			// count overflow
+			//  reset count, but maintain mean and variance
+			_M2 = _M2 / _count;
+			_M2_accum.zero();
+
+			_count = 1;
+
+		} else {
+			_count++;
+		}
+
+		// mean
+		//  accumulates the mean of the entire dataset
+		//  use Kahan summation because delta can be very small compared to the mean
+		const matrix::Vector<Type, N> delta{new_value - _mean};
+		{
+			const matrix::Vector<Type, N> y = (delta / _count) - _mean_accum;
+			const matrix::Vector<Type, N> t = _mean + y;
+			_mean_accum = (t - _mean) - y;
+			_mean = t;
+		}
+
+		if (!_mean.isAllFinite()) {
+			reset();
+			return false;
+		}
+
+		// covariance
+		//  Kahan summation (upper triangle only)
+		{
+			// eg C(x,y) += dx * (y - mean_y)
+			matrix::SquareMatrix<Type, N> m2_change{};
+
+			for (size_t r = 0; r < N; r++) {
+				for (size_t c = r; c < N; c++) {
+					m2_change(r, c) = delta(r) * (new_value(c) - _mean(c));
+				}
+			}
+
+			for (size_t r = 0; r < N; r++) {
+				for (size_t c = r; c < N; c++) {
+
+					const Type y = m2_change(r, c) - _M2_accum(r, c);
+					const Type t = _M2(r, c) + y;
+					_M2_accum(r, c) = (t - _M2(r, c)) - y;
+
+					_M2(r, c) = t;
+				}
+
+				// protect against floating point precision causing negative variances
+				if (_M2(r, r) < 0) {
+					_M2(r, r) = 0;
+				}
+			}
+
+			// make symmetric
+			for (size_t r = 0; r < N; r++) {
+				for (size_t c = r + 1; c < N; c++) {
+					_M2(c, r) = _M2(r, c);
+				}
+			}
+		}
+
+		if (!_M2.isAllFinite()) {
+			reset();
+			return false;
+		}
+
+		return valid();
+	}
+
+	bool valid() const { return _count > 2; }
+	auto count() const { return _count; }
+
+	void reset()
+	{
+		_count = 0;
+		_mean.zero();
+		_M2.zero();
+
+		_mean_accum.zero();
+		_M2_accum.zero();
+	}
+
+	// Retrieve the mean, variance and sample variance
+	matrix::Vector<Type, N> mean() const { return _mean; }
+	matrix::Vector<Type, N> variance() const { return _M2.diag() / (_count - 1); }
+	matrix::SquareMatrix<Type, N> covariance() const { return _M2 / (_count - 1); }
+
+	Type covariance(int x, int y) const { return _M2(x, y) / (_count - 1); }
+
+private:
+
+	matrix::Vector<Type, N> _mean{};
+	matrix::Vector<Type, N> _mean_accum{};  ///< kahan summation algorithm accumulator for mean
+
+	matrix::SquareMatrix<Type, N> _M2{};
+	matrix::SquareMatrix<Type, N> _M2_accum{};    ///< kahan summation algorithm accumulator for M2
+
+	uint16_t _count{0};
+};
+
+} // namespace math
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <random>
+#include <lib/matrix/matrix/math.hpp>
+#include "WelfordMeanVector.hpp"
+
+using namespace math;
+using matrix::Vector3f;
+
+TEST(WelfordMeanVectorTest, NoisySignal)
+{
+	const float std_dev = 3.f;
+	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);
+	WelfordMeanVector<float, 3> welford{};
+
+	for (int i = 0; i < 1000; i++) {
+		const float noisy_value = standard_normal_distribution(random_generator);
+		welford.update(Vector3f(noisy_value, noisy_value - 1.f, noisy_value + 1.f));
+	}
+
+	EXPECT_TRUE(welford.valid());
+	const Vector3f mean = welford.mean();
+	const Vector3f var = welford.variance();
+
+	const float var_real = std_dev * std_dev;
+
+	EXPECT_NEAR(mean(0), 0.f, 0.7f);
+	EXPECT_NEAR(mean(1), -1.f, 0.7f);
+	EXPECT_NEAR(mean(2), 1.f, 0.7f);
+	EXPECT_NEAR(var(0), var_real, 0.1f);
+	EXPECT_NEAR(var(1), var_real, 0.1f);
+	EXPECT_NEAR(var(2), var_real, 0.1f);
+}