diff options
| author | Martin Miller | 2017-03-27 14:23:43 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-03-27 14:23:43 -0500 |
| commit | e0734384252675e2a37f4d9287184cc48ab68b05 (patch) | |
| tree | 15d69d56bda52fcaa968d8edf87b2a6b4e0adb79 /src/state.h | |
| parent | d98f18feaf37f40389968715106938b6e58a94c5 (diff) | |
| download | refslam-e0734384252675e2a37f4d9287184cc48ab68b05.zip refslam-e0734384252675e2a37f4d9287184cc48ab68b05.tar.gz | |
EKF is working
Diffstat (limited to 'src/state.h')
| -rw-r--r-- | src/state.h | 7 |
1 files changed, 3 insertions, 4 deletions
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 ======================================= */ |