More filter testing.

test/filter.cpp

@@ -22,6 +22,12 @@ int main()
     kalman_correct<float, 6, 5>(P, C, R, r, dx, beta);
 
     dx.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);
+
 
     return 0;
 }