Fix testing mechanism.

test/filter.cpp

@@ -1,7 +1,7 @@
-#include <assert.h>
 #include <stdio.h>
 
 #include <matrix/filter.hpp>
+#include "test_macros.hpp"
 
 using namespace matrix;
 
@@ -21,13 +21,9 @@ int main()
     float beta = 0;
     kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
 
-    dx.T().print();
-    printf("beta: %g\n", beta);
-
     float data_check[] = {0.5,1,1.5,2,2.5,0};
     Vector<float, n_x> dx_check(data_check);
-    assert(dx == dx_check);
-
+    TEST(isEqual(dx, dx_check));
 
     return 0;
 }