diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp
index 9b67c78ea9..780f6a8f13 100644
--- a/src/lib/matrix/matrix/helper_functions.hpp
+++ b/src/lib/matrix/matrix/helper_functions.hpp
@@ -133,15 +133,7 @@ Type wrap_2pi(Type x)
 template<typename Type>
 Type unwrap(const Type last_angle, const Type new_angle)
 {
-	// wrap the last angle in [-pi,pi]
-	const Type last_angle_wrapped = matrix::detail::wrap_floating(last_angle, -Type(M_PI), Type(M_PI));
-
-	// use the shortest distance
-	Type delta = new_angle - last_angle_wrapped;
-	delta += ((delta < -Type(M_PI)) - (delta > Type(M_PI))) * Type(2 *
-			M_PI); // adds or subtracts 2*pi if delta out of range
-
-	return delta + last_angle;
+	return last_angle + wrap_pi(new_angle - last_angle);
 }
 
 template<typename T>