#ifndef state_INC #define state_INC #include #include #include #include "body.h" #include "camera.h" #include "feature.h" #include "types.h" #define MAXFEATURES 30 #define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once using Eigen::Matrix; using Eigen::MatrixXd; using Eigen::Dynamic; using Eigen::Vector3d; using Eigen::Quaterniond; using std::cout; using std::cerr; using std::endl; /* * ===================================================================================== * Class: State * Description: * ===================================================================================== */ class State { public: /* ==================== LIFECYCLE ======================================= */ State (); /* constructor */ /* ==================== ACCESSORS ======================================= */ bool exists(int id); MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd Q(double dt); Matrix Pxx(); Matrix L(); /* ==================== MUTATORS ======================================= */ void accelerometer_bias(const Vector3d &b); void enu(const UTM &utm); void feature_update( const std::vector & z, const Quaterniond &q); void initializePi(int i, const Matrix &Pi); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); void position_covariance(const Matrix &p); std::list::iterator removeFeature(std::list::iterator &i, int j); void velocity_covariance(const Matrix &p); void vel(const Matrix &v); void update ( const Matrix &z); /* ==================== OPERATORS ======================================= */ void unicsv(); protected: /* ==================== METHODS ======================================= */ /* ==================== DATA MEMBERS ======================================= */ private: /* ==================== METHODS ======================================= */ void addFeatures(std::vector &F, const Quaterniond &q); void expandP(); void shrinkP( int i ); /* ==================== DATA MEMBERS ======================================= */ Body *body; //Matrix P; Matrix P; char utm_c; int utm_i; std::list features; }; /* ----- end of class State ----- */ #endif /* ----- #ifndef state_INC ----- */