From e972a0a111e35132f87e77d2bac096ba4e2728b2 Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Mon, 9 Nov 2015 19:52:55 -0500
Subject: [PATCH] Made kalman filter correction function usable.

---
 matrix/filter.hpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/matrix/filter.hpp b/matrix/filter.hpp
index e202bb8827..53fc99d92a 100644
--- a/matrix/filter.hpp
+++ b/matrix/filter.hpp
@@ -6,17 +6,20 @@ namespace matrix {
 
 template<typename Type, size_t M, size_t N>
 void kalman_correct(
-    const Matrix<Type, M, M> & P,
+    const SquareMatrix<Type, M> & P,
     const Matrix<Type, N, M> & C,
-    const Matrix<Type, N, N> & R,
+    const SquareMatrix<Type, N> & R,
     const Vector<Type, N> &r,
     Vector<Type, M> & dx,
+    SquareMatrix<Type, M> & dP,
     float & beta
 )
 {
     SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
-    dx = P*C.T()*S_I*r;
+    Matrix<Type, M, N> K = P*C.T()*S_I;
+    dx = K*r;
     beta = Scalar<Type>(r.T()*S_I*r);
+    dP = K*C*P*(-1);
 }
 
 }; // namespace matrix