diff --git a/include/gz/math/Filter.hh b/include/gz/math/Filter.hh index 5a1ddeb3..5b8dcec8 100644 --- a/include/gz/math/Filter.hh +++ b/include/gz/math/Filter.hh @@ -87,7 +87,7 @@ namespace gz::math /// \return The filter's current output. public: const T& Process(const T &_x) { - this->y0 = a0 * _x + b1 * this->y0; + this->y0 = static_cast(a0 * _x + b1 * this->y0); return this->y0; } @@ -199,11 +199,12 @@ namespace gz::math /// \return The filter's current output. public: virtual const T& Process(const T &_x) { - this->y0 = this->a0 * _x + + this->y0 = static_cast( + this->a0 * _x + this->a1 * this->x1 + this->a2 * this->x2 - this->b1 * this->y1 - - this->b2 * this->y2; + this->b2 * this->y2); this->x2 = this->x1; this->x1 = _x; diff --git a/src/Filter_TEST.cc b/src/Filter_TEST.cc index aa9e9264..dc3e5238 100644 --- a/src/Filter_TEST.cc +++ b/src/Filter_TEST.cc @@ -51,6 +51,9 @@ TEST(FilterTest, OnePoleQuaternion) EXPECT_EQ(filterB.Process(math::Quaterniond(0.1, 0.2, 0.3)), math::Quaterniond(0.98841, 0.0286272, 0.0885614, 0.119929)); + + filterA.Set(math::Quaterniond(0.707, 0, 0.707, 0)); + EXPECT_EQ(filterA.Value(), math::Quaterniond(0.707, 0, 0.707, 0)); } ///////////////////////////////////////////////// @@ -67,6 +70,9 @@ TEST(FilterTest, OnePoleVector3) EXPECT_EQ(filterB.Process(math::Vector3d(0.1, 0.2, 0.3)), math::Vector3d(0.089113, 0.178226, 0.267339)); + + filterA.Set(math::Vector3d(1, 2, 3)); + EXPECT_EQ(filterA.Value(), math::Vector3d(1, 2, 3)); } ///////////////////////////////////////////////// @@ -81,6 +87,9 @@ TEST(FilterTest, Biquad) filterA.Fc(0.3, 1.4, 0.1); EXPECT_DOUBLE_EQ(filterA.Process(10.25), 0.96057152402651302); + // Multi-step Process calls to exercise state update + EXPECT_DOUBLE_EQ(filterA.Process(5.0), 2.2809792005709335); + EXPECT_DOUBLE_EQ(filterA.Process(2.0), 2.2786852100645718); math::BiQuad filterB(4.3, 10.6); EXPECT_NEAR(filterB.Value(), 0.0, 1e-10); @@ -102,4 +111,40 @@ TEST(FilterTest, BiquadVector3) EXPECT_EQ(filterB.Value(), math::Vector3d(0, 0, 0)); EXPECT_EQ(filterB.Process(math::Vector3d(0.1, 20.3, 33.45)), math::Vector3d(0.031748, 6.44475, 10.6196)); + + filterA.Set(math::Vector3d(4, 5, 6)); + EXPECT_EQ(filterA.Value(), math::Vector3d(4, 5, 6)); + + filterA.Fc(0.5, 2.0); + EXPECT_EQ(filterA.Process(math::Vector3d(1.0, 2.0, 3.0)), + math::Vector3d(3.25, 4.25, 5.25)); + + filterA.Fc(0.5, 2.0, 0.2); + EXPECT_EQ(filterA.Process(math::Vector3d(1.0, 2.0, 3.0)), + math::Vector3d(19.0 / 7.0, 26.0 / 7.0, 33.0 / 7.0)); } + +///////////////////////////////////////////////// +TEST(FilterTest, TemplateTypes) +{ + // OnePole template instantiations + math::OnePole filterF(0.6f, 1.4f); + EXPECT_FLOAT_EQ(filterF.Process(2.5f), 2.330771f); + filterF.Set(1.5f); + EXPECT_FLOAT_EQ(filterF.Value(), 1.5f); + + math::OnePole filterI(1, 2); + filterI.Set(10); + EXPECT_EQ(filterI.Value(), 10); + + // BiQuad template instantiations + math::BiQuad biquadF(4.3f, 10.6f); + EXPECT_NEAR(biquadF.Value(), 0.0f, 1e-6f); + biquadF.Set(3.5f); + EXPECT_FLOAT_EQ(biquadF.Value(), 3.5f); + + math::BiQuad biquadI(1, 2); + biquadI.Set(20); + EXPECT_EQ(biquadI.Value(), 20); +} + diff --git a/src/python_pybind11/test/Filter_TEST.py b/src/python_pybind11/test/Filter_TEST.py index cc157d31..34c9fe3b 100644 --- a/src/python_pybind11/test/Filter_TEST.py +++ b/src/python_pybind11/test/Filter_TEST.py @@ -14,8 +14,12 @@ import unittest from gz.math import BiQuadd +from gz.math import BiQuadf +from gz.math import BiQuadi from gz.math import BiQuadVector3 from gz.math import OnePoled +from gz.math import OnePolef +from gz.math import OnePolei from gz.math import OnePoleQuaternion from gz.math import OnePoleVector3 from gz.math import Quaterniond @@ -51,6 +55,9 @@ def test_one_pole_quaternion(self): Quaterniond(0.98841, 0.0286272, 0.0885614, 0.119929)) + filter_a.set(Quaterniond(0.707, 0, 0.707, 0)) + self.assertAlmostEqual(filter_a.value(), Quaterniond(0.707, 0, 0.707, 0)) + def test_one_pole_vector3(self): filter_a = OnePoleVector3() self.assertAlmostEqual(filter_a.value(), Vector3d(0, 0, 0)) @@ -64,6 +71,9 @@ def test_one_pole_vector3(self): self.assertAlmostEqual(filter_b.process(Vector3d(0.1, 0.2, 0.3)), Vector3d(0.089113, 0.178226, 0.267339)) + filter_a.set(Vector3d(1, 2, 3)) + self.assertEqual(filter_a.value(), Vector3d(1, 2, 3)) + def test_biquad(self): filter_a = BiQuadd() self.assertAlmostEqual(filter_a.value(), 0.0, delta=1e-10) @@ -74,6 +84,8 @@ def test_biquad(self): filter_a.fc(0.3, 1.4, 0.1) self.assertAlmostEqual(filter_a.process(10.25), 0.96057152402651302) + self.assertAlmostEqual(filter_a.process(5.0), 2.2809792005709335) + self.assertAlmostEqual(filter_a.process(2.0), 2.2786852100645718) filter_b = BiQuadd(4.3, 10.6) self.assertAlmostEqual(filter_b.value(), 0.0, delta=1e-10) @@ -93,6 +105,37 @@ def test_biquad_vector3(self): self.assertEqual(filter_b.process(Vector3d(0.1, 20.3, 33.45)), Vector3d(0.031748, 6.44475, 10.6196)) + filter_a.set(Vector3d(4, 5, 6)) + self.assertEqual(filter_a.value(), Vector3d(4, 5, 6)) + + filter_a.fc(0.5, 2.0) + self.assertEqual(filter_a.process(Vector3d(1.0, 2.0, 3.0)), + Vector3d(3.25, 4.25, 5.25)) + + filter_a.fc(0.5, 2.0, 0.2) + self.assertEqual(filter_a.process(Vector3d(1.0, 2.0, 3.0)), + Vector3d(19.0 / 7.0, 26.0 / 7.0, 33.0 / 7.0)) + + def test_template_types(self): + filter_f = OnePolef(0.6, 1.4) + self.assertAlmostEqual(filter_f.process(2.5), 2.330771, delta=1e-5) + filter_f.set(1.5) + self.assertAlmostEqual(filter_f.value(), 1.5) + + filter_i = OnePolei(1, 2) + filter_i.set(10) + self.assertEqual(filter_i.value(), 10) + + biquad_f = BiQuadf(4.3, 10.6) + self.assertAlmostEqual(biquad_f.value(), 0.0, delta=1e-6) + biquad_f.set(3.5) + self.assertAlmostEqual(biquad_f.value(), 3.5) + + biquad_i = BiQuadi(1, 2) + biquad_i.set(20) + self.assertEqual(biquad_i.value(), 20) + if __name__ == '__main__': unittest.main() +