Added filtering test.

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 : */

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;

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