diff --git a/CMakeLists.txt b/CMakeLists.txt
index 10040a237c..622559c982 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -22,7 +22,7 @@ set(CMAKE_CXX_FLAGS
 	-Wall
 	-Weffc++
 	-Werror
-	-Wfatal-errors
+	#-Wfatal-errors
 	)
 
 if (COVERALLS)
diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp
index 0bbd8d601a..56bd5d37cc 100644
--- a/matrix/SquareMatrix.hpp
+++ b/matrix/SquareMatrix.hpp
@@ -209,6 +209,15 @@ SquareMatrix<Type, M> eye() {
     return m;
 }
 
+template<typename Type, size_t M>
+SquareMatrix<Type, M> diag(Vector<Type, M> d) {
+    SquareMatrix<Type, M> m;
+    for (size_t i=0; i<M; i++) {
+        m(i,i) = d(i);
+    }
+    return m;
+}
+
 }; // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp
index b039b0d10a..6b77cfc7ff 100644
--- a/matrix/Vector.hpp
+++ b/matrix/Vector.hpp
@@ -25,6 +25,16 @@ public:
     {
     }
 
+    Vector(const Vector<Type, M> & other) :
+        Matrix<Type, M, 1>(other)
+    {
+    }
+
+    Vector(const Matrix<Type, M, 1> & other) :
+        Matrix<Type, M, 1>(other)
+    {
+    }
+
     inline Type operator()(size_t i) const
     {
         const Matrix<Type, M, 1> &v = *this;
diff --git a/matrix/filtering.hpp b/matrix/filter.hpp
similarity index 62%
rename from matrix/filtering.hpp
rename to matrix/filter.hpp
index f70d329ec8..d04d70c699 100644
--- a/matrix/filtering.hpp
+++ b/matrix/filter.hpp
@@ -2,17 +2,21 @@
 
 #include "matrix.hpp"
 
+namespace matrix {
+
 template<typename Type, size_t M, size_t N>
 void kalman_correct(
 		const Matrix<Type, M, M> & P,
-		const Matrix<Type, N, N> & R,
 		const Matrix<Type, N, M> & C,
+		const Matrix<Type, N, N> & R,
 		const Vector<Type, N> &r,
 		Vector<Type, M> & dx,
-		float beta
+		float & beta
 		)
 {
-	SuareMatrix<Type, N> S_I = SquarMatrix<Type, N>(C*P*C.T() + R).I();
+	SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
 	dx = P*C.T()*S_I*r;
-	beta = Scalar(r.T()*S_I*r);
+	beta = Scalar<Type>(r.T()*S_I*r);
 }
+
+}; // namespace matrix
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4c37fe51c9..53702b78c1 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -11,6 +11,7 @@ set(tests
 	vector
 	vector3
 	euler
+	filter
 	)
 
 foreach(test ${tests})
diff --git a/test/filter.cpp b/test/filter.cpp
new file mode 100644
index 0000000000..71745c133a
--- /dev/null
+++ b/test/filter.cpp
@@ -0,0 +1,27 @@
+#include "filter.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+template class Vector<float, 5>;
+
+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;
+    Matrix<float, n_y, n_x> C;
+    C(0,0) = 1;
+    Vector<float, n_y> r;
+    r.setZero();
+    r(0) = 1;
+
+    Vector<float, n_x> dx;
+    float beta = 0;
+    kalman_correct<float, 6, 5>(P, C, R, r, dx, beta);
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
diff --git a/test/quaternion.cpp b/test/quaternion.cpp
index 1e0ffd3424..018f03a6ad 100644
--- a/test/quaternion.cpp
+++ b/test/quaternion.cpp
@@ -18,6 +18,12 @@ int main()
     assert(q(2) == 0);
     assert(q(3) == 0);
 
+    q = Quatf(1,2,3,4);
+    assert(q(0) == 1);
+    assert(q(1) == 2);
+    assert(q(2) == 3);
+    assert(q(3) == 4);
+
     q = Quatf(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
     assert(q(0) == 0.1825742f);
     assert(q(1) == 0.3651484f);