From e0734384252675e2a37f4d9287184cc48ab68b05 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Mon, 27 Mar 2017 14:23:43 -0500
Subject: EKF is working

---
 src/state.h | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

(limited to 'src/state.h')

diff --git a/src/state.h b/src/state.h
index ea8cfef..4001216 100644
--- a/src/state.h
+++ b/src/state.h
@@ -9,7 +9,7 @@
 #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
 #define FULLS // Comment out to treat each measurement independently.
 using Eigen::Dynamic;
@@ -48,7 +48,7 @@ class State
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias(const Vector3d &b);
         void pos(const UTM &utm);
-        void feature_update( const std::vector<measurement_t> & z, const Quaterniond &q);
+        void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q);
         void initializePi(int i, const Matrix<double,3,3> &Pi);
         void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
                 const std::vector<measurement_t> &z, const Quaterniond &q);
@@ -59,7 +59,6 @@ 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 Quaterniond &q, const std::vector<measurement_t> &z );
 
         /* ====================  OPERATORS     ======================================= */
         Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q);
@@ -73,7 +72,7 @@ class State
     private:
         /* ====================  METHODS       ======================================= */
         void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q);
-        void expandP();
+        void expandP ( const Matrix3d &Pi);
         void shrinkP( int i );
 
         /* ====================  DATA MEMBERS  ======================================= */
-- 
cgit v1.1