From 2c0ce68acd031b67fdffaea3b2e50cca4f487633 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Tue, 4 Apr 2017 16:10:26 -0500 Subject: Full state mostly implemented. STATESIZE 9 works just as it does for experiment04032017 tag. The quaternion covariance is not being set correctly yet. --- src/state.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/state.h') 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 &z ); MatrixXd blockSI( const std::vector &z ); + Quaterniond qhat(); #else MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd H ( const Quaterniond &q, const std::vector &z ); @@ -74,6 +74,8 @@ class State const std::vector &z); void Pkk1 ( const Vector3d &ang, const double dt); void ransacUpdate(std::vector &z); + void quaternion_covariance(); + void qhat(const Quaterniond &q); #else void handle_measurements( const std::vector & z, const Quaterniond &q); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, -- cgit v1.1