diff --git a/fmd.hh b/fmd.hh
index 900d7b5..d0c2687 100644
--- a/fmd.hh
+++ b/fmd.hh
@@ -15,6 +15,14 @@ class FMD1
 	typedef typename complex_type::value_type value_type;
 	value_type prev;
 	value_type scale;
+	value_type unwrap(value_type angle)
+	{
+		if (angle < -DSP::Const<value_type>::Pi())
+			return angle + DSP::Const<value_type>::TwoPi();
+		if (angle > DSP::Const<value_type>::Pi())
+			return angle - DSP::Const<value_type>::TwoPi();
+		return angle;
+	}
 public:
 	constexpr FMD1() : prev(0), scale(0)
 	{
@@ -27,6 +35,7 @@ public:
 	{
 		value_type phase = arg(input);
 		value_type delta = phase - prev;
+		delta = unwrap(delta);
 		prev = phase;
 		return scale * delta;
 	}