1 files changed, 47 insertions, 11 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 9a2a6b3..f2344f0 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -60,26 +60,36 @@ State::update ( const Matrix<double,1,1> &z )
 }
 
 void
-State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q )
+State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q)
 {
     std::vector<measurement_t> featuresToAdd;
     for (auto i=z.begin(); i!=z.end(); ++i) {
         if (exists(i->id)) {
-            printf("Exists!\n");
+            ;
         } else {
             featuresToAdd.push_back(*i);
         }
     }
+    addFeatures( featuresToAdd, q);
 
-    // Expand P matrix
-    expandP(featuresToAdd.size());
+    return ;
+}		/* -----  end of method State::feature_update  ----- */
 
+void
+State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
+{
+    // Expand P matrix all at once
+    expandP(F.size());
+    //
     // Add new features
-    for (auto i=featuresToAdd.begin(); i!=featuresToAdd.end(); ++i) {
+    for (auto i=F.begin(); i!=F.end(); ++i) {
+        // Estimate initial depth
+        Vector3d xib;
+
         // Create feature
-        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q);
+        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44);
         features.push_back(f);
-
+        
         // Initialize P values
         int j=features.size();
         Matrix<double,3,3> Pi;
@@ -87,7 +97,7 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &
         initializePi(j,Pi);
     }
     return ;
-}		/* -----  end of method State::feature_update  ----- */
+}		/* -----  end of method State::addFeatures  ----- */
 
 /*
  *--------------------------------------------------------------------------------------
@@ -109,6 +119,30 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi )
     return ;
 }		/* -----  end of method State::initializePi  ----- */
 
+MatrixXd
+State::F ( const Quaterniond &q, const Vector3d &w, double dt )
+{
+    Vector3d v;
+    v = body->vel();
+    // Allocate matrix F
+    MatrixXd f;
+    int rows = 9 + 3*features.size();
+    f = MatrixXd::Zero(rows,rows);
+
+    // Set body F
+    f.topLeftCorner<9,9>() = body->F(w,q,dt);
+    // Set Fxi Fyi
+    { // limit i's scope
+        auto i = features.begin();
+        for (int row=9; row<rows; row+=3,++i) {
+            f.block<3,9>(row,0) = (*i)->Fx(dt);
+            f.block<3,3>(row,row) = (*i)->Fy(v,w,dt);
+        }
+    }
+
+    return f ;
+}		/* -----  end of method State::F  ----- */
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  State
@@ -119,10 +153,12 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi )
 void
 State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
 {
-    Matrix<double,9,9> F,Q;
+    Matrix<double,9,9> Q;
     Q = body->Q(dt);
-    F = body->F(ang,q,dt);
-    P = F*P*F.transpose()+Q;
+    MatrixXd f;
+    f = F ( q, ang, dt );
+    //cout << f << endl;
+    P = f*P*f.transpose()+Q;
     // Enforce symmetry
     P = 0.5*(P+P.transpose());
     return ;