From c0727bbe1404e788b4ae82f6c6d99e105ecfacdf Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sat, 18 Mar 2017 18:09:39 -0500
Subject: Add fake z kalman update

---
 src/state.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index 8aadb64..294b522 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -17,6 +17,19 @@
  */
 #include "state.h"
 
+void 
+State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, 
+        const Matrix<double,1,9> H, const Matrix<double,1,1> &h,
+        const Matrix<double,1,1> &z, const Matrix<double,1,1> &R)
+{
+    Matrix<double,1,1> S;
+    Matrix<double,9,1> K;
+    S = H*P*H.transpose() + R;
+    K = P*H.transpose()*S.inverse();
+    P = (Matrix<double,9,9>::Identity()-K*H)*P;
+    P = 0.5*(P+P.transpose());
+    X += K*(z-h);
+}
 
 /*
  *--------------------------------------------------------------------------------------
-- 
cgit v1.1