a/tracking: Add an integer version of the low-pass filter.

src/xrt/auxiliary/CMakeLists.txt

@@ -101,6 +101,7 @@ add_library(
 	tracking/t_imu.h
 	tracking/t_lowpass_vector.hpp
 	tracking/t_lowpass.hpp
+	tracking/t_lowpass_integer.hpp
 	tracking/t_tracking.h
 	)
 target_link_libraries(

src/xrt/auxiliary/tracking/t_lowpass_integer.hpp

@@ -0,0 +1,169 @@
+// Copyright 2019, 2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Low-pass IIR filter for integers
+ * @author Ryan Pavlik <ryan.pavlik@collabora.com>
+ * @ingroup aux_tracking
+ */
+
+#pragma once
+
+#ifndef __cplusplus
+#error "This header is C++-only."
+#endif
+
+#include "util/u_time.h"
+
+#include "math/m_mathinclude.h"
+#include "math/m_rational.hpp"
+#include <cmath>
+#include <type_traits>
+#include <stdexcept>
+#include <cassert>
+
+namespace xrt::auxiliary::tracking {
+
+
+namespace detail {
+	/*!
+	 * The shared implementation (between vector and scalar versions) of an integer
+	 * IIR/exponential low-pass filter.
+	 */
+	template <typename Value, typename Scalar> struct IntegerLowPassIIR
+	{
+		// For fixed point, you'd need more bits of data storage. See
+		// https://www.embeddedrelated.com/showarticle/779.php
+		static_assert(std::is_integral<Scalar>::value,
+		              "Filter is designed only for integral values. Use the other one for floats.");
+
+		/*!
+		 * Constructor
+		 *
+		 * @param alpha_ The alpha value used to blend between new input and existing state. Larger values mean
+		 * more influence from new input. @p alpha_.isBetweenZeroAndOne() must be true.
+		 *
+		 * @param val The value to initialize the filter with. Does not
+		 * affect the filter itself: only seen if you access state
+		 * before initializing the filter with the first sample.
+		 */
+		explicit IntegerLowPassIIR(math::Rational<Scalar> alpha_, Value const &val)
+		    : state(val), alpha(alpha_.withNonNegativeDenominator())
+		{
+			if (!alpha.isBetweenZeroAndOne()) {
+				throw std::invalid_argument("Alpha must be between zero and one.");
+			}
+		}
+
+		/*!
+		 * Reset the filter to just-created state.
+		 */
+		void
+		reset(Value const &val) noexcept
+		{
+			state = val;
+			initialized = false;
+		}
+
+		/*!
+		 * Filter a sample, with an optional weight.
+		 *
+		 * @param sample The value to filter
+		 * @param weight An optional value between 0 and 1. The smaller
+		 * this value, the less the current sample influences the filter
+		 * state. For the first call, this is always assumed to be 1.
+		 */
+		void
+		addSample(Value const &sample, math::Rational<Scalar> weight = math::Rational<Scalar>::simplestUnity())
+		{
+			if (!initialized) {
+				initialized = true;
+				state = sample;
+				return;
+			}
+			math::Rational<Scalar> weightedAlpha = alpha * weight;
+
+			math::Rational<Scalar> oneMinusWeightedAlpha = weightedAlpha.complement();
+
+			Value scaledStateNumerator = oneMinusWeightedAlpha.numerator * state;
+			Value scaledSampleNumerator = weightedAlpha.numerator * sample;
+
+			// can't use the re-arranged update from the float impl because we might be unsigned.
+			state = (scaledStateNumerator + scaledSampleNumerator) / weightedAlpha.denominator;
+		}
+
+		Value state;
+		math::Rational<Scalar> alpha;
+		bool initialized{false};
+	};
+} // namespace detail
+
+/*!
+ * A very simple integer low-pass filter, using a "one-pole infinite impulse response"
+ * design (one-pole IIR), aka exponential filter.
+ *
+ * Configurable in scalar type.
+ */
+template <typename Scalar> class IntegerLowPassIIRFilter
+{
+public:
+	/*!
+	 * Constructor
+	 *
+	 * @note Taking alpha, not a cutoff frequency, here, because it's easier with the rational math.
+	 *
+	 * @param alpha The alpha value used to blend between new input and existing state. Larger values mean
+	 * more influence from new input. @ref math::Rational::isBetweenZeroAndOne() must be true for @p alpha.
+	 */
+	explicit IntegerLowPassIIRFilter(math::Rational<Scalar> alpha) noexcept : impl_(alpha, 0)
+	{
+		assert(alpha.isBetweenZeroAndOne());
+	}
+
+	/*!
+	 * Reset the filter to just-created state.
+	 */
+	void
+	reset() noexcept
+	{
+		impl_.reset(0);
+	}
+
+	/*!
+	 * Filter a sample, with an optional weight.
+	 *
+	 * @param sample The value to filter
+	 * @param weight An optional value between 0 and 1. The smaller this
+	 * value, the less the current sample influences the filter state. For
+	 * the first call, this is always assumed to be 1 regardless of what you pass.
+	 */
+	void
+	addSample(Scalar sample, math::Rational<Scalar> weight = math::Rational<Scalar>::simplestUnity())
+	{
+		impl_.addSample(sample, weight);
+	}
+
+	/*!
+	 * Access the filtered value.
+	 */
+	Scalar
+	getState() const noexcept
+	{
+		return impl_.state;
+	}
+
+
+	/*!
+	 * Access whether we have initialized state.
+	 */
+	bool
+	isInitialized() const noexcept
+	{
+		return impl_.initialized;
+	}
+
+private:
+	detail::IntegerLowPassIIR<Scalar, Scalar> impl_;
+};
+
+} // namespace xrt::auxiliary::tracking

tests/CMakeLists.txt

@@ -17,6 +17,7 @@ foreach(
 	tests_id_ringbuffer
 	tests_input_transform
 	tests_json
+	tests_lowpass_int
 	tests_pacing
 	tests_quatexpmap
 	tests_rational
@@ -33,5 +34,6 @@ endforeach()
 target_link_libraries(tests_cxx_wrappers PRIVATE xrt-interfaces)
 target_link_libraries(tests_history_buf PRIVATE aux_math)
 target_link_libraries(tests_input_transform PRIVATE st_oxr xrt-interfaces xrt-external-openxr)
+target_link_libraries(tests_lowpass_int PRIVATE aux_tracking)
 target_link_libraries(tests_quatexpmap PRIVATE aux_math)
 target_link_libraries(tests_rational PRIVATE aux_math)

tests/tests_lowpass_int.cpp

@@ -0,0 +1,69 @@
+// Copyright 2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief Integer low pass filter tests.
+ * @author Ryan Pavlik <ryan.pavlik@collabora.com>
+ */
+
+#include <tracking/t_lowpass_integer.hpp>
+
+#include "catch/catch.hpp"
+
+
+using xrt::auxiliary::math::Rational;
+using xrt::auxiliary::tracking::IntegerLowPassIIRFilter;
+static constexpr uint16_t InitialState = 300;
+
+TEMPLATE_TEST_CASE("t_lowpass_integer", "", int32_t, uint32_t)
+{
+	IntegerLowPassIIRFilter<TestType> filter(Rational<TestType>{1, 2});
+
+	CHECK_FALSE(filter.isInitialized());
+
+	filter.addSample(InitialState);
+	CHECK(filter.getState() == InitialState);
+	CHECK(filter.isInitialized());
+
+	auto prev = filter.getState();
+	SECTION("Increase")
+	{
+		constexpr auto newTarget = InitialState * 2;
+		for (int i = 0; i < 20; ++i) {
+			filter.addSample(newTarget);
+			REQUIRE(filter.isInitialized());
+			// not going to exceed this
+			if (prev == newTarget || prev == newTarget - 1) {
+				REQUIRE(filter.getState() == prev);
+			} else {
+				REQUIRE(filter.getState() > prev);
+				prev = filter.getState();
+			}
+		}
+	}
+
+	SECTION("Decrease")
+	{
+		constexpr auto newTarget = InitialState / 2;
+		for (int i = 0; i < 20; ++i) {
+
+			filter.addSample(newTarget);
+			REQUIRE(filter.isInitialized());
+			if (prev == newTarget) {
+				REQUIRE(filter.getState() == newTarget);
+			} else {
+				REQUIRE(filter.getState() < prev);
+				prev = filter.getState();
+			}
+		}
+	}
+	SECTION("Stay Same")
+	{
+		for (int i = 0; i < 20; ++i) {
+
+			filter.addSample(InitialState);
+			REQUIRE(filter.isInitialized());
+			REQUIRE(filter.getState() == InitialState);
+		}
+	}
+}