diff --git a/src/pyrecest/filters/piecewise_constant_filter.py b/src/pyrecest/filters/piecewise_constant_filter.py
index fa71a980a..760a76ff4 100644
--- a/src/pyrecest/filters/piecewise_constant_filter.py
+++ b/src/pyrecest/filters/piecewise_constant_filter.py
@@ -5,7 +5,7 @@
 import pyrecest.backend
 
 # pylint: disable=no-name-in-module,no-member
-from pyrecest.backend import array, dot, floor, pi, zeros
+from pyrecest.backend import array, dot, floor, mod, pi, zeros
 from pyrecest.distributions import AbstractCircularDistribution
 from pyrecest.distributions.circle.piecewise_constant_distribution import (
     PiecewiseConstantDistribution,
@@ -194,7 +194,7 @@ def calculate_system_matrix_numerically(L, a, noise_distribution):
                 r2 = PiecewiseConstantDistribution.right_border(j + 1, L)
 
                 def integrand(x, w, _l2=l2, _r2=r2):
-                    ax = a(x, w)
+                    ax = mod(a(x, w), 2.0 * pi)
                     in_interval = 1.0 if _l2 <= ax < _r2 else 0.0
                     return float(noise_distribution.pdf(array([w]))) * in_interval
 
@@ -249,7 +249,7 @@ def calculate_measurement_matrix_numerically(L, l_meas, h, noise_distribution):
                 r2 = PiecewiseConstantDistribution.right_border(j + 1, L)
 
                 def integrand(x, v, _l1=l1, _r1=r1):
-                    hx = h(x, v)
+                    hx = mod(h(x, v), 2.0 * pi)
                     in_interval = 1.0 if _l1 <= hx < _r1 else 0.0
                     return float(noise_distribution.pdf(array([v]))) * in_interval
 
diff --git a/tests/filters/test_piecewise_constant_filter_circular_wrapping.py b/tests/filters/test_piecewise_constant_filter_circular_wrapping.py
new file mode 100644
index 000000000..caaa7017e
--- /dev/null
+++ b/tests/filters/test_piecewise_constant_filter_circular_wrapping.py
@@ -0,0 +1,44 @@
+import unittest
+
+import numpy.testing as npt
+import pyrecest.backend
+
+from pyrecest.backend import array
+from pyrecest.distributions.circle.piecewise_constant_distribution import (
+    PiecewiseConstantDistribution,
+)
+from pyrecest.filters.piecewise_constant_filter import PiecewiseConstantFilter
+
+
+@unittest.skipUnless(
+    pyrecest.backend.__backend_name__ == "numpy",  # pylint: disable=no-member
+    "SciPy numerical integration regression is NumPy-only.",
+)
+class TestPiecewiseConstantFilterCircularWrapping(unittest.TestCase):
+    def setUp(self):
+        self.uniform_noise = PiecewiseConstantDistribution(array([1.0]))
+
+    def test_system_matrix_wraps_additive_model_outputs(self):
+        matrix = PiecewiseConstantFilter.calculate_system_matrix_numerically(
+            2,
+            lambda x, w: x + w,
+            self.uniform_noise,
+        )
+
+        npt.assert_allclose(matrix, 0.5, atol=2e-4)
+        npt.assert_allclose(matrix.sum(axis=0), 1.0, atol=2e-4)
+
+    def test_measurement_matrix_wraps_additive_model_outputs(self):
+        matrix = PiecewiseConstantFilter.calculate_measurement_matrix_numerically(
+            2,
+            2,
+            lambda x, v: x + v,
+            self.uniform_noise,
+        )
+
+        npt.assert_allclose(matrix, 0.5, atol=2e-4)
+        npt.assert_allclose(matrix.sum(axis=0), 1.0, atol=2e-4)
+
+
+if __name__ == "__main__":
+    unittest.main()