Added kalman filter.

matrix/Matrix.hpp

@@ -295,6 +295,13 @@ public:
 
 };
 
+template<typename Type, size_t M, size_t N>
+Matrix<Type, M, N> & zero() {
+    Matrix<Type, M, N> m;
+    m.setZero();
+    return m;
+}
+
 typedef Matrix<float, 3, 3> Matrix3f;
 
 }; // namespace matrix

matrix/SquareMatrix.hpp

@@ -202,6 +202,13 @@ public:
 
 typedef SquareMatrix<float, 3> SquareMatrix3f;
 
+template<typename Type, size_t M>
+SquareMatrix<Type, M> eye() {
+    SquareMatrix<Type, M> m;
+    m.setIdentity();
+    return m;
+}
+
 }; // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/filtering.hpp

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "matrix.hpp"
+
+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 Vector<Type, N> &r,
+		Vector<Type, M> & dx,
+		float beta
+		)
+{
+	SuareMatrix<Type, N> S_I = SquarMatrix<Type, N>(C*P*C.T() + R).I();
+	dx = P*C.T()*S_I*r;
+	beta = Scalar(r.T()*S_I*r);
+}

matrix/matrix.hpp

@@ -0,0 +1,9 @@
+#pragma once
+
+#include "Matrix.hpp"
+#include "Vector.hpp"
+#include "Vector3.hpp"
+#include "Euler.hpp"
+#include "Dcm.hpp"
+#include "Scalar.hpp"
+#include "SquareMatrix.hpp"

test/dcm.cpp

@@ -1,15 +1,19 @@
-#include "Dcm.hpp"
 #include <assert.h>
 #include <stdio.h>
 
+#include "matrix.hpp"
+
 using namespace matrix;
 
 int main()
 {
     Dcmf dcm;
-    Quatf q = Quatf(dcm);
+    Quatf q(1,0,0,0);
+    dcm = Dcmf(q);
+    Matrix3f I = eye<float, 3>();
+    dcm = Dcmf(q);
     Eulerf e = Eulerf(dcm);
     return 0;
-}
+};
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */