From b7b8835f7075c2dcfeed8a272662a7a44168b092 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sat, 25 Mar 2017 18:33:46 -0500
Subject: Methods added to State to get H

Added a method to return an H matrix for the provided measurements. This
should be a versatile method that can work for the full H update or the
indpendent update depending on what measurements are passed.

Some helper methods were also added for determining a features
location in the state vector and for return a feature pointer by id.
---
 src/state.h | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

(limited to 'src/state.h')

diff --git a/src/state.h b/src/state.h
index db45ba2..43342a7 100644
--- a/src/state.h
+++ b/src/state.h
@@ -11,6 +11,7 @@
 
 #define MAXFEATURES 30
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
+#define FULLS // Comment out to treat each measurement independently.
 using Eigen::Matrix;
 using Eigen::MatrixXd;
 using Eigen::Dynamic;
@@ -33,7 +34,10 @@ class State
 
         /* ====================  ACCESSORS     ======================================= */
         bool exists(int id);
+        int rowById(int id);
+        Feature *featureById(int id);
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
+        MatrixXd H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z );
         MatrixXd Q(double dt);
         Matrix<double,9,9> Pxx();
         Matrix<double,Dynamic,6> L();
@@ -50,7 +54,7 @@ class State
         std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j);
         void velocity_covariance(const Matrix<double,3,3> &p);
         void vel(const Matrix<double,3,1> &v);
-        void update ( const Matrix<double,1,1> &z);
+        void update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z );
 
         /* ====================  OPERATORS     ======================================= */
         void unicsv();
-- 
cgit v1.1