Add fake z kalman update

1 files changed, 13 insertions, 0 deletions

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);
+}
 
 /*
  *--------------------------------------------------------------------------------------