summaryrefslogtreecommitdiff
path: root/src/state.h
diff options
context:
space:
mode:
authorMartin Miller2017-03-25 16:16:02 -0500
committerMartin Miller2017-03-25 16:16:02 -0500
commitec60ec9eba475fcfbdfc5d7bd157095affb7d66f (patch)
tree8e45e498b7dcc861d6975279c12c51ce46dbd890 /src/state.h
parent1c233f6fd05502585928e058d5d2c0ff5e23881a (diff)
downloadrefslam-ec60ec9eba475fcfbdfc5d7bd157095affb7d66f.zip
refslam-ec60ec9eba475fcfbdfc5d7bd157095affb7d66f.tar.gz
Update State::motionModel and addFeatures
Add the feature motion model update to the state method. It is implemented in two ways: one is the for loop that operates on each feature instance directly, which is how it has been implemented in the past. The second method is to compose a large L matrix for all features and compute the per feature dx in one go. It is expected that the second method is faster, but it is not yet tested for speed.
Diffstat (limited to 'src/state.h')
-rw-r--r--src/state.h10
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;