Work on filter.

CMakeLists.txt

@@ -22,6 +22,7 @@ set(CMAKE_CXX_FLAGS
 	-Wall
 	-Weffc++
 	-Werror
+	-std=c++11
 	#-Wfatal-errors
 	)
 

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_x> P = eye<float, n_x>();
-    SquareMatrix<float, n_y> R = eye<float, n_y>()*0.1;
+    SquareMatrix<float, n_y> R = eye<float, n_y>();
     Matrix<float, n_y, n_x> C;
-    C(0,0) = 1;
+    C.setIdentity();
-    Vector<float, n_y> r;
+    float data[] = {1,2,3,4,5};
-    r.setZero();
+    Vector<float, n_y> r(data);
-    r(0) = 1;
 
     Vector<float, n_x> dx;
     float beta = 0;
     kalman_correct<float, 6, 5>(P, C, R, r, dx, beta);
+
+    dx.print();
+
     return 0;
 }