From eddc55827a3c8c9ca0547620f8bcf1e788c75376 Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Wed, 4 Nov 2015 20:58:56 -0500
Subject: [PATCH] More filter testing.

---
 matrix/filter.hpp | 20 ++++++++++----------
 test/filter.cpp   |  6 ++++++
 2 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/matrix/filter.hpp b/matrix/filter.hpp
index d04d70c699..666db0926e 100644
--- a/matrix/filter.hpp
+++ b/matrix/filter.hpp
@@ -6,17 +6,17 @@ namespace matrix {
 
 template<typename Type, size_t M, size_t N>
 void kalman_correct(
-		const Matrix<Type, M, M> & P,
-		const Matrix<Type, N, M> & C,
-		const Matrix<Type, N, N> & R,
-		const Vector<Type, N> &r,
-		Vector<Type, M> & dx,
-		float & beta
-		)
+    const Matrix<Type, M, M> & P,
+    const Matrix<Type, N, M> & C,
+    const Matrix<Type, N, N> & R,
+    const Vector<Type, N> &r,
+    Vector<Type, M> & dx,
+    float & beta
+)
 {
-	SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
-	dx = P*C.T()*S_I*r;
-	beta = Scalar<Type>(r.T()*S_I*r);
+    SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
+    dx = P*C.T()*S_I*r;
+    beta = Scalar<Type>(r.T()*S_I*r);
 }
 
 }; // namespace matrix
diff --git a/test/filter.cpp b/test/filter.cpp
index b88b61244a..8e723e8c63 100644
--- a/test/filter.cpp
+++ b/test/filter.cpp
@@ -22,6 +22,12 @@ int main()
     kalman_correct<float, 6, 5>(P, C, R, r, dx, beta);
 
     dx.print();
+    printf("beta: %g\n", beta);
+
+    float data_check[] = {0.5,1,1.5,2,2.5,0};
+    Vector<float, n_x> dx_check(data_check);
+    assert(dx == dx_check);
+
 
     return 0;
 }