Work on filter.

test/filter.cpp

@@ -10,17 +10,19 @@ int main()
 {
     const size_t n_x = 6;
     const size_t n_y = 5;
-    SquareMatrix<float, n_x> P = eye<float, n_x>()*0.1;
-    SquareMatrix<float, n_y> R = eye<float, n_y>()*0.1;
+    SquareMatrix<float, n_x> P = eye<float, n_x>();
+    SquareMatrix<float, n_y> R = eye<float, n_y>();
     Matrix<float, n_y, n_x> C;
-    C(0,0) = 1;
-    Vector<float, n_y> r;
-    r.setZero();
-    r(0) = 1;
+    C.setIdentity();
+    float data[] = {1,2,3,4,5};
+    Vector<float, n_y> r(data);
 
     Vector<float, n_x> dx;
     float beta = 0;
     kalman_correct<float, 6, 5>(P, C, R, r, dx, beta);
+
+    dx.print();
+
     return 0;
 }