diff options
Diffstat (limited to 'src/body.h')
| -rw-r--r-- | src/body.h | 31 |
1 files changed, 24 insertions, 7 deletions
@@ -9,6 +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 */ using Eigen::Matrix; @@ -25,31 +26,42 @@ class Body { public: /* ==================== LIFECYCLE ======================================= */ - Body (){}; /* constructor */ + 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); +#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); +#endif Vector3d ned(); Vector3d vel(); /* ==================== MUTATORS ======================================= */ void accelerometer_bias( const Vector3d &b); - void asVector(const Matrix<double,9,1> &m); void clamp(); void pos( const UTM &utm); - void dx( const Matrix<double,9,1> &del); void vel(const Matrix<double,3,1> &v); /* ==================== OPERATORS ======================================= */ - Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt); - Matrix<double,1,9> H(); Matrix<double,1,1> h(); void motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); - Matrix<double,9,9> Q(double dt); Matrix<double,1,1> R(); - Matrix<double,1,1> S(const Matrix<double,9,9> &Pxx); Matrix<double,3,3> skewSymmetric(const Vector3d &x); + Matrix<double,4,4> omega(const Vector3d &x); void unicsv(); protected: @@ -61,9 +73,14 @@ class Body /* ==================== METHODS ======================================= */ /* ==================== DATA MEMBERS ======================================= */ +#ifdef FULLSTATE + Matrix<double,13,1> X; +#else Matrix<double,9,1> X; +#endif char utm_c; int utm_i; + Vector3d gravity_world; }; /* ----- end of class Body ----- */ |