Add feature handling to state

1 files changed, 11 insertions, 1 deletions

diff --git a/src/state.h b/src/state.h
index c91c1ae..8a9d318 100644
--- a/src/state.h
+++ b/src/state.h
@@ -1,11 +1,16 @@
 #ifndef  state_INC
 #define  state_INC
 #include <Eigen/Dense>
+#include <list>
+#include <vector>
+
 #include "body.h"
+#include "feature.h"
 #include "types.h"
 
 #define MAXFEATURES 30
 using Eigen::Matrix;
+using Eigen::MatrixXd;
 using Eigen::Dynamic;
 using Eigen::Vector3d;
 using Eigen::Quaterniond;
@@ -22,19 +27,22 @@ class State
         State ();                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
+        bool exists(int id);
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias(const Vector3d &b);
         void enu(const UTM &utm);
+        void feature_update( const std::vector<measurement_t> & z, const Quaterniond &q);
+        void initializePi(int i, const Matrix<double,3,3> &Pi);
         void motionModel(const Vector3d &acc, const Vector3d &ang,
                 const Quaterniond &q, const double dt);
         void position_covariance(const Matrix<double,3,3> &p);
         void velocity_covariance(const Matrix<double,3,3> &p);
         void vel(const Matrix<double,3,1> &v);
         void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
+        void update ( const Matrix<double,1,1> &z);
 
         /* ====================  OPERATORS     ======================================= */
-        void update ( const Matrix<double,1,1> &z);
         void unicsv();
 
     protected:
@@ -44,12 +52,14 @@ class State
 
     private:
         /* ====================  METHODS       ======================================= */
+        void expandP(int Nnew);
 
         /* ====================  DATA MEMBERS  ======================================= */
         Body *body;
         Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P;
         char utm_c;
         int utm_i;
+        std::list<Feature *> features;
 
 
 }; /* -----  end of class State  ----- */