diff options
| author | Martin Miller | 2017-04-04 16:10:26 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-04-04 16:10:26 -0500 |
| commit | 2c0ce68acd031b67fdffaea3b2e50cca4f487633 (patch) | |
| tree | cbaeca5d911fefaca0468c6aa469a5bfc7cf0c6d /src/state.h | |
| parent | 0c8912c5dc1ec3c752f8648ca2a8181275ba2654 (diff) | |
| download | refslam-2c0ce68acd031b67fdffaea3b2e50cca4f487633.zip refslam-2c0ce68acd031b67fdffaea3b2e50cca4f487633.tar.gz | |
Full state mostly implemented.
STATESIZE 9 works just as it does for experiment04032017 tag. The
quaternion covariance is not being set correctly yet.
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, |