From ec60ec9eba475fcfbdfc5d7bd157095affb7d66f Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sat, 25 Mar 2017 16:16:02 -0500
Subject: 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.
---
 src/state.h | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

(limited to 'src/state.h')

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;
-- 
cgit v1.1