From b2e9e1e3c72abe3a1a096bc712225ed06d6a98d5 Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Wed, 4 Nov 2015 18:58:29 -0500
Subject: [PATCH] Added kalman filter.

---
 matrix/Matrix.hpp       |  7 +++++++
 matrix/SquareMatrix.hpp |  7 +++++++
 matrix/filtering.hpp    | 18 ++++++++++++++++++
 matrix/matrix.hpp       |  9 +++++++++
 test/dcm.cpp            | 10 +++++++---
 5 files changed, 48 insertions(+), 3 deletions(-)
 create mode 100644 matrix/filtering.hpp
 create mode 100644 matrix/matrix.hpp

diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index dc72f85921..5103626282 100644
--- a/matrix/Matrix.hpp
+++ b/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
diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp
index 059a2d5cc5..0bbd8d601a 100644
--- a/matrix/SquareMatrix.hpp
+++ b/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 : */
diff --git a/matrix/filtering.hpp b/matrix/filtering.hpp
new file mode 100644
index 0000000000..f70d329ec8
--- /dev/null
+++ b/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);
+}
diff --git a/matrix/matrix.hpp b/matrix/matrix.hpp
new file mode 100644
index 0000000000..e7ea9ca1d4
--- /dev/null
+++ b/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"
diff --git a/test/dcm.cpp b/test/dcm.cpp
index 2779d46113..a01c8e05db 100644
--- a/test/dcm.cpp
+++ b/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 : */