From b2cba6dc96c22f35fcf0df75527868cc36e93760 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sun, 26 Mar 2017 16:33:20 -0500
Subject: Fully implemented EKF.

Still needs to be debugged as the result is way off, but it is all
there.
---
 src/body.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

(limited to 'src/body.cpp')

diff --git a/src/body.cpp b/src/body.cpp
index 83838a5..ae43e58 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -28,6 +28,11 @@ void
 Body::update ( const Matrix<double,9,1> &dx )
 {
     X += dx;
+    if (X[2]<-1.) {
+        X[2]=-1.;
+    } else if (X[2]>-0.2) {
+        X[2]=-0.2;
+    }
     return ;
 }		/* -----  end of method Body::update  ----- */
 
@@ -84,7 +89,7 @@ Matrix<double,1,1>
 Body::R()
 {
     Matrix<double,1,1> R;
-    R << 1e-3;
+    R << 1e-6;
     return R;
 }
 
-- 
cgit v1.1