Fully implemented EKF.

Still needs to be debugged as the result is way off, but it is all
there.

1 files changed, 6 insertions, 1 deletions

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;
 }