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/body.cpp | 15 ++++++++++++---
 1 file changed, 12 insertions(+), 3 deletions(-)

(limited to 'src/body.cpp')

diff --git a/src/body.cpp b/src/body.cpp
index 4741911..c4d8a82 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -19,6 +19,14 @@
 #include <Eigen/Dense>
 #include "body.h"
 
+Matrix<double,1,1> 
+Body::R()
+{
+    Matrix<double,1,1> R;
+    R << 1e-8;
+    return R;
+}
+
 Matrix<double,9,9>
 Body::Q (double dt)
 {
@@ -56,11 +64,12 @@ Body::H ( Matrix<double,1,9> &H )
  * Description:  Writes the predicted measurement vector into h.
  *--------------------------------------------------------------------------------------
  */
-void
-Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h )
+Matrix<double,1,1>
+Body::h ( const Matrix<double,9,1> &X)
 {
+    Matrix<double,1,1> h;
     h[0] = X[2];
-    return ;
+    return h;
 }		/* -----  end of method Body::h_hat  ----- */
 
 /*
-- 
cgit v1.1