diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp
index dd2b28ea1b..9b67c78ea9 100644
--- a/src/lib/matrix/matrix/helper_functions.hpp
+++ b/src/lib/matrix/matrix/helper_functions.hpp
@@ -123,6 +123,27 @@ Type wrap_2pi(Type x)
 	return wrap(x, Type(0), Type(M_TWOPI));
 }
 
+/**
+ * Unwrap angles
+ *
+ * @param[in] last_angle Last unwrapped angle [rad]
+ * @param[in] new_angle New angle in [-pi, pi] [rad]
+ * @return New unwrapped angle [rad]
+ */
+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;
+}
+
 template<typename T>
 int sign(T val)
 {
diff --git a/src/lib/matrix/test/CMakeLists.txt b/src/lib/matrix/test/CMakeLists.txt
index 1eb71f9634..42a581b8d3 100644
--- a/src/lib/matrix/test/CMakeLists.txt
+++ b/src/lib/matrix/test/CMakeLists.txt
@@ -44,3 +44,4 @@ foreach(test_name ${tests})
 endforeach()
 
 px4_add_unit_gtest(SRC sparseVector.cpp)
+px4_add_unit_gtest(SRC unwrap.cpp)
diff --git a/src/lib/matrix/test/unwrap.cpp b/src/lib/matrix/test/unwrap.cpp
new file mode 100644
index 0000000000..7495a0976b
--- /dev/null
+++ b/src/lib/matrix/test/unwrap.cpp
@@ -0,0 +1,62 @@
+#include <matrix/math.hpp>
+#include <gtest/gtest.h>
+
+using namespace matrix;
+
+TEST(Unwrap, UnwrapFloats)
+{
+	const float M_TWO_PI_F = float(M_PI * 2);
+
+	float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
+	float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
+
+	for (int i = 0; i < 6; i++) {
+		unwrapped_angles[i] *= M_TWO_PI_F;
+		wrapped_angles[i] *= M_TWO_PI_F;
+	}
+
+	// positive unwrapping
+	float last_angle = wrapped_angles[0];
+
+	for (int i = 1; i < 6; i++) {
+		last_angle = unwrap(last_angle, wrapped_angles[i]);
+		EXPECT_FLOAT_EQ(last_angle, unwrapped_angles[i]);
+	}
+
+	// negative unwrapping
+	last_angle = -wrapped_angles[0];
+
+	for (int i = 1; i < 6; i++) {
+		last_angle = unwrap(last_angle, -wrapped_angles[i]);
+		EXPECT_FLOAT_EQ(last_angle, -unwrapped_angles[i]);
+	}
+}
+
+TEST(Unwrap, UnwrapDoubles)
+{
+	const double M_TWO_PI = M_PI * 2;
+
+	double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
+	double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
+
+	for (int i = 0; i < 6; i++) {
+		unwrapped_angles[i] *= M_TWO_PI;
+		wrapped_angles[i] *= M_TWO_PI;
+	}
+
+	// positive unwrapping
+	double last_angle = wrapped_angles[0];
+
+	for (int i = 1; i < 6; i++) {
+		last_angle = unwrap(last_angle, wrapped_angles[i]);
+		EXPECT_DOUBLE_EQ(last_angle, unwrapped_angles[i]);
+	}
+
+	// negative unwrapping
+	last_angle = -wrapped_angles[0];
+
+	for (int i = 1; i < 6; i++) {
+		last_angle = unwrap(last_angle, -wrapped_angles[i]);
+		EXPECT_DOUBLE_EQ(last_angle, -unwrapped_angles[i]);
+	}
+}