1 files changed, 84 insertions, 0 deletions
diff --git a/src/state.cpp b/src/state.cpp
index 84aa565..9a2a6b3 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -59,6 +59,56 @@ State::update ( const Matrix<double,1,1> &z )
     body->update(dx);
 }
 
+void
+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);
+        }
+    }
+
+    // Expand P matrix
+    expandP(featuresToAdd.size());
+
+    // Add new features
+    for (auto i=featuresToAdd.begin(); i!=featuresToAdd.end(); ++i) {
+        // Create feature
+        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q);
+        features.push_back(f);
+
+        // Initialize P values
+        int j=features.size();
+        Matrix<double,3,3> Pi;
+        Pi = f->P0();
+        initializePi(j,Pi);
+    }
+    return ;
+}		/* -----  end of method State::feature_update  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: initializePi
+ * Description:  Zeros out the off-diagonal blocks and sets the Pii block to Pi.
+ *--------------------------------------------------------------------------------------
+ */
+    void
+State::initializePi ( int i, const Matrix<double,3,3> &Pi )
+{
+    int pt=9+3*(i-1);
+    std::cout << pt << std::endl;
+    std::cout << P.cols() << std::endl;
+    P.block(pt,0,3,P.cols()) = MatrixXd::Zero(3,P.cols());
+    P.block(0,pt,P.rows(),3) = MatrixXd::Zero(P.rows(),3);
+    P.block<3,3>(pt,pt) = Pi;
+
+    return ;
+}		/* -----  end of method State::initializePi  ----- */
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  State
@@ -115,3 +165,37 @@ State::accelerometer_bias ( const Vector3d &b)
     return ;
 }		/* -----  end of method State::accelerometer_bias  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: exists
+ * Description:  Tests if the id is a current feature.
+ *--------------------------------------------------------------------------------------
+ */
+bool
+State::exists ( int id )
+{
+    for (auto i=features.begin(); i!=features.end(); ++i) {
+        if ((*i)->id()==id) return true;
+    }
+    return false;
+}		/* -----  end of method State::exists  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: expandP
+ * Description:  Grows P so that Nnew new features can be added. This could mean
+ * explicitly resizing the P matrix or P being a block of a preallocated matrix.
+ *--------------------------------------------------------------------------------------
+ */
+void
+State::expandP ( int Nnew )
+{
+    int curRows, newRows;
+    curRows = P.rows();
+    newRows = curRows + 3*Nnew;
+    P.conservativeResize(newRows,newRows);
+    return ;
+}		/* -----  end of method State::expandP  ----- */
+