diff options
Diffstat (limited to 'src/state.h')
| -rw-r--r-- | src/state.h | 10 |
1 files changed, 7 insertions, 3 deletions
diff --git a/src/state.h b/src/state.h index 1d8dd63..67b235f 100644 --- a/src/state.h +++ b/src/state.h @@ -9,7 +9,8 @@ #include "feature.h" #include "types.h" -#define MAXFEATURES 30 +#define MAXFEATURES 50 +#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once using Eigen::Matrix; using Eigen::MatrixXd; using Eigen::Dynamic; @@ -34,6 +35,8 @@ class State bool exists(int id); MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd Q(double dt); + Matrix<double,9,9> Pxx(); + Matrix<double,Dynamic,6> L(); /* ==================== MUTATORS ======================================= */ void accelerometer_bias(const Vector3d &b); @@ -58,12 +61,13 @@ class State private: /* ==================== METHODS ======================================= */ - void expandP(int Nnew); + void expandP(); void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q); /* ==================== DATA MEMBERS ======================================= */ Body *body; - Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P; + //Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P; + Matrix<double, Dynamic, Dynamic> P; char utm_c; int utm_i; std::list<Feature *> features; |