diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp
index 0bbad666c9..559a6c1b7b 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp
@@ -47,69 +47,63 @@ using matrix::Vector3f;
 class LowPassFilter2pVector3fTest : public ::testing::Test
 {
 public:
+	void runSimulatedFilter(const Vector3f &signal_freq_hz, const Vector3f &phase_delay_deg, const Vector3f &gain_db);
+
 	math::LowPassFilter2pVector3f _lpf{800.f, 30.f};
-	const float _sample_freq = 1000.f;
-	const float _cutoff_freq = 80.f;
 
 	const float _epsilon_near = 10e-3f;
 };
 
+void LowPassFilter2pVector3fTest::runSimulatedFilter(const Vector3f &signal_freq_hz, const Vector3f &phase_delay_deg,
+		const Vector3f &gain_db)
+{
+	const Vector3f phase_delay = phase_delay_deg * M_PI_F / 180.f;
+	const Vector3f omega = 2.f * M_PI_F * signal_freq_hz;
+	Vector3f gain;
+
+	for (int i = 0; i < 3; i++) {
+		gain(i) = powf(10.f, gain_db(i) / 20.f);
+	}
+
+	const float dt = 1.f / _lpf.get_sample_freq();
+
+	float t = 0.f;
+
+	for (int i = 0; i < 1000; i++) {
+		Vector3f input{0.f, sinf(omega(1) * t), -sinf(omega(2) * t)};
+		Vector3f output_expected{0.f,
+					 gain(1) *sinf(omega(1) * t - phase_delay(1)),
+					 -gain(2) *sinf(omega(2) * t - phase_delay(2))};
+		Vector3f out = _lpf.apply(input);
+		t = i * dt;
+
+		// Let some time for the filter to settle
+		if (i > 30) {
+			EXPECT_EQ(out(0), 0.f);
+			EXPECT_NEAR(out(1), output_expected(1), _epsilon_near);
+			EXPECT_NEAR(out(2), output_expected(2), _epsilon_near);
+		}
+	}
+}
+
 TEST_F(LowPassFilter2pVector3fTest, setGet)
 {
-	_lpf.set_cutoff_frequency(_sample_freq, _cutoff_freq);
-	EXPECT_EQ(_lpf.get_sample_freq(), _sample_freq);
-	EXPECT_EQ(_lpf.get_cutoff_freq(), _cutoff_freq);
+	const float sample_freq = 1000.f;
+	const float cutoff_freq = 80.f;
+
+	_lpf.set_cutoff_frequency(sample_freq, cutoff_freq);
+	EXPECT_EQ(_lpf.get_sample_freq(), sample_freq);
+	EXPECT_EQ(_lpf.get_cutoff_freq(), cutoff_freq);
 }
 
-TEST_F(LowPassFilter2pVector3fTest, belowCutoff)
+TEST_F(LowPassFilter2pVector3fTest, belowAndAboveCutoff)
 {
-	_lpf.set_cutoff_frequency(_sample_freq, _cutoff_freq);
+	const float sample_freq = 1000.f;
+	const float cutoff_freq = 80.f;
+	_lpf.set_cutoff_frequency(sample_freq, cutoff_freq);
 
-	const float signal_freq = 10.f;
-	const float omega = 2.f * M_PI_F * signal_freq;
-	const float phase_delay = 10.4f * M_PI_F / 180.f; // Given by simulation
-	const float dt = 1.f / _sample_freq;
-
-	float t = 0.f;
-
-	for (int i = 0; i < 1000; i++) {
-		float input = sinf(omega * t);
-		float output_expected = sinf(omega * t - phase_delay);
-		Vector3f out = _lpf.apply(Vector3f(0.f, input, -input));
-		t = i * dt;
-
-		// Let some time for the filter to settle
-		if (i > 30) {
-			EXPECT_EQ(out(0), 0.f);
-			EXPECT_NEAR(out(1), output_expected, _epsilon_near);
-			EXPECT_NEAR(out(2), -output_expected, _epsilon_near);
-		}
-	}
-}
-
-TEST_F(LowPassFilter2pVector3fTest, aboveCutoff)
-{
-	_lpf.set_cutoff_frequency(_sample_freq, _cutoff_freq);
-
-	const float signal_freq = 100.f;
-	const float omega = 2.f * M_PI_F * signal_freq;
-	const float phase_delay = 108.5f * M_PI_F / 180.f; // Given by simulation
-	const float gain = 0.52f; // = -5.66 dB, given by simulation
-	const float dt = 1.f / _sample_freq;
-
-	float t = 0.f;
-
-	for (int i = 0; i < 1000; i++) {
-		float input = sinf(omega * t);
-		float output_expected = gain * sinf(omega * t - phase_delay);
-		Vector3f out = _lpf.apply(Vector3f(0.f, input, -input));
-		t = i * dt;
-
-		// Let some time for the filter to settle
-		if (i > 30) {
-			EXPECT_EQ(out(0), 0.f);
-			EXPECT_NEAR(out(1), output_expected, _epsilon_near);
-			EXPECT_NEAR(out(2), -output_expected, _epsilon_near);
-		}
-	}
+	const Vector3f signal_freq_hz{0.f, 10.f, 100.f};
+	const Vector3f phase_delay_deg = Vector3f{0.f, 10.4f, 108.5f}; // Given by simulation
+	const Vector3f gain_db{0.f, 0.f, -5.66f}; // given by simulation
+	runSimulatedFilter(signal_freq_hz, phase_delay_deg, gain_db);
 }