diff options
Diffstat (limited to 'src/body.h')
| -rw-r--r-- | src/body.h | 38 |
1 files changed, 16 insertions, 22 deletions
@@ -9,7 +9,7 @@ //#define DOCLAMP #define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */ #define MINHEIGHT -0.3 /* */ -#define FULLSTATE /* Uncomment to estimate quaternion */ +#define STATESIZE 13 using Eigen::Matrix; @@ -29,27 +29,21 @@ class Body Body (){ gravity_world << 0.,0.,9.80655; }; /* constructor */ /* ==================== ACCESSORS ======================================= */ -#ifdef FULLSTATE - Matrix<double,13,1> asVector(); - void asVector(const Matrix<double,13,1> &m); - void dx( const Matrix<double,13,1> &del); - Matrix<double,13,13> F(const Vector3d &ang, const Quaterniond &q, double dt); - Matrix<double,1,13> H(); - Matrix<double,13,13> Q(double dt); - Matrix<double,1,1> S(const Matrix<double,13,13> &Pxx); +#if STATESIZE==13 + Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt); Quaterniond qhat(); #else - Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt); - Matrix<double,9,1> asVector(); - void asVector(const Matrix<double,9,1> &m); - void dx( const Matrix<double,9,1> &del); - Matrix<double,1,9> H(); - Matrix<double,9,9> Q(double dt); - Matrix<double,1,1> S(const Matrix<double,9,9> &Pxx); + Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt); #endif + Matrix<double,STATESIZE,1> asVector(); + void asVector(const Matrix<double,STATESIZE,1> &m); + void dx( const Matrix<double,STATESIZE,1> &del); + Matrix<double,1,STATESIZE> H(); + Matrix<double,STATESIZE,STATESIZE> Q(double dt); + Matrix<double,1,1> S(const Matrix<double,STATESIZE,STATESIZE> &Pxx); Vector3d ned(); Vector3d vel(); - + Vector3d accelerometer_bias( ); /* ==================== MUTATORS ======================================= */ void accelerometer_bias( const Vector3d &b); void clamp(); @@ -58,8 +52,12 @@ class Body /* ==================== OPERATORS ======================================= */ Matrix<double,1,1> h(); +#if STATESIZE==13 + void motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt); +#else void motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); +#endif Matrix<double,1,1> R(); Matrix<double,3,3> skewSymmetric(const Vector3d &x); Matrix<double,4,4> omega(const Vector3d &x); @@ -74,11 +72,7 @@ class Body /* ==================== METHODS ======================================= */ /* ==================== DATA MEMBERS ======================================= */ -#ifdef FULLSTATE - Matrix<double,13,1> X; -#else - Matrix<double,9,1> X; -#endif + Matrix<double,STATESIZE,1> X; char utm_c; int utm_i; Vector3d gravity_world; |