diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp
index f7e737ef42..d42d0c949a 100644
--- a/matrix/helper_functions.hpp
+++ b/matrix/helper_functions.hpp
@@ -6,8 +6,9 @@
 
 namespace matrix
 {
+
 template<typename Type>
-float wrap_pi(Type x)
+Type wrap_pi(Type x)
 {
     if (!isfinite(x)) {
         return x;
@@ -27,4 +28,4 @@ float wrap_pi(Type x)
 }
 
 
-};
\ No newline at end of file
+};
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 76357d890c..7351b3ae7c 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -13,6 +13,7 @@ set(tests
 	filter
 	integration
 	squareMatrix
+	helper
 	)
 
 add_custom_target(test_build)
diff --git a/test/helper.cpp b/test/helper.cpp
new file mode 100644
index 0000000000..68bdced00b
--- /dev/null
+++ b/test/helper.cpp
@@ -0,0 +1,18 @@
+#include <assert.h>
+#include <stdio.h>
+
+#include <matrix/helper_functions.hpp>
+
+using namespace matrix;
+
+
+int main()
+{
+    assert(fabs(wrap_pi(4.0) - (4.0 - 2*M_PI)) < 1e-5);
+    assert(fabs(wrap_pi(-4.0) - (-4.0 + 2*M_PI)) < 1e-5);
+    assert(fabs(wrap_pi(3.0) - (3.0)) < 1e-3);
+    wrap_pi(NAN);
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */