Fully implemented EKF.

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

1 files changed, 5 insertions, 5 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 57ebbfa..998be20 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -21,7 +21,7 @@ State::State ( )
 {
     body = new Body;
     P = Matrix<double,Dynamic,Dynamic>::Zero(9,9);
-    P.block(6,6,3,3) = 1e-6*Matrix3d::Identity();
+    P.block(6,6,3,3) = 1e-9*Matrix3d::Identity();
     return ;
 }		/* -----  end of method State::State  ----- */
 
@@ -204,9 +204,7 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
             fi = featureById(z[i].id);
             Matrix<double,6,1> hi, zi;
             hi = fi->h(body->ned(), q);
-            zi.segment<2>(0) = z[i].source.segment<2>(0);
-            zi.segment<2>(2) = fi->initialView();
-            zi.segment<2>(4) = z[i].reflection.segment<2>(0);
+            zi = fi->measurement_vector(z[i].source, z[i].reflection);
             y.segment<6>(row) = zi-hi;
             row += 6;
         } else if (mt==MONO) {
@@ -238,6 +236,7 @@ State::update ( const Quaterniond &q, const std::vector<measurement_t> &z )
     MatrixXd S;
     S = h*P*h.transpose() + R(z);
     kalmanUpdate(h,S,z,q);
+    cout << features.size() << endl;
 
 #else      /* -----  not FULLS  ----- */
     
@@ -280,7 +279,8 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
     for (auto i=F.begin(); i!=F.end(); ++i) {
         if (features.size()>MAXFEATURES) break;
         // Create feature
-        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44);
+        Vector3d pos = body->ned();
+        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, pos[2]);
         if (!f->sane()) {
             delete f;
             continue;