diff options
Diffstat (limited to 'src/state.h')
| -rw-r--r-- | src/state.h | 8 |
1 files changed, 6 insertions, 2 deletions
diff --git a/src/state.h b/src/state.h index 2a18e36..3a90217 100644 --- a/src/state.h +++ b/src/state.h @@ -12,11 +12,12 @@ #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::Dynamic; using Eigen::Matrix; using Eigen::MatrixXd; -using Eigen::Dynamic; -using Eigen::Vector3d; using Eigen::Quaterniond; +using Eigen::Vector2d; +using Eigen::Vector3d; using std::cout; using std::cerr; using std::endl; @@ -49,6 +50,8 @@ class State void enu(const UTM &utm); void feature_update( 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); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); @@ -59,6 +62,7 @@ class State void update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ); /* ==================== OPERATORS ======================================= */ + Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q); void unicsv(); protected: |