diff --git a/include/testlib.hpp b/include/testlib.hpp
index 9498df1..fbc0d17 100644
--- a/include/testlib.hpp
+++ b/include/testlib.hpp
@@ -14,10 +14,11 @@
 #ifndef __TESTLIB__
 #define __TESTLIB__
 
+#include "utils.hpp"
+
 #include <armadillo>
 #include <string>
-
-#include "utils.hpp"
+#include <type_traits>
 
 /** @def ASSERT(expr)
  *  @brief A prettier assertion function.
@@ -55,4 +56,24 @@ void m_assert(bool expr, std::string expr_str, std::string func,
  *  @return bool
  * */
 bool close_to(arma::vec &a, arma::vec &b, double tol = 1e-8);
+
+/** @brief Test that all elements fulfill the condition.
+ *
+ *  @param expr The boolean expression to apply to each element
+ *  @param M The matrix/vector to iterate over 
+ *
+ *  @return bool
+ * */
+template <class T,
+          class = typename std::enable_if<std::is_arithmetic<T>::value>::type>
+static bool assert_each(std::function<bool(T)> expr, arma::Mat<T> &M)
+{
+    for (size_t i = 0; i < M.n_elem; i++) {
+        if (!expr(M(i))) {
+            return false;
+        }
+    }
+    return true;
+}
+
 #endif
diff --git a/src/testlib.cpp b/src/testlib.cpp
index 126f5bc..4a4d9ea 100644
--- a/src/testlib.cpp
+++ b/src/testlib.cpp
@@ -12,11 +12,14 @@
 
 #include "testlib.hpp"
 
+#include <type_traits>
+
 static void print_message(std::string msg)
 {
     if (msg.size() > 0) {
         std::cout << "message: " << msg << "\n\n";
-    } else {
+    }
+    else {
         std::cout << "\n";
     }
 }
@@ -30,7 +33,8 @@ void m_assert(bool expr, std::string expr_str, std::string f, std::string file,
     if (expr) {
         std::cout << "\x1B[32mOK\033[0m\n";
         print_message(msg);
-    } else {
+    }
+    else {
         std::cout << "\x1B[31mFAIL\033[0m\n";
         print_message(msg);
         std::cout << file << " " << line << ": Assertion \"" << expr_str