diff options
Diffstat (limited to 'src/state.h')
| -rw-r--r-- | src/state.h | 4 |
1 files changed, 3 insertions, 1 deletions
diff --git a/src/state.h b/src/state.h index f330b67..b04ae9c 100644 --- a/src/state.h +++ b/src/state.h @@ -15,7 +15,6 @@ //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define DORANSAC /* */ //#define INLIERTEST /* */ -#define STATESIZE 13 /* Set to 13 for quaternion estimation, or 9 for quaternion as input */ using Eigen::Dynamic; using Eigen::Matrix; using Eigen::MatrixXd; @@ -48,6 +47,7 @@ class State MatrixXd F(const Vector3d &w, double dt); MatrixXd H ( const std::vector<measurement_t> &z ); MatrixXd blockSI( const std::vector<measurement_t> &z ); + Quaterniond qhat(); #else MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z ); @@ -74,6 +74,8 @@ class State const std::vector<measurement_t> &z); void Pkk1 ( const Vector3d &ang, const double dt); void ransacUpdate(std::vector<measurement_t> &z); + void quaternion_covariance(); + void qhat(const Quaterniond &q); #else void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, |