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/body.cpp | |
| 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/body.cpp')
| -rw-r--r-- | src/body.cpp | 19 |
1 files changed, 14 insertions, 5 deletions
diff --git a/src/body.cpp b/src/body.cpp index 819bd01..f909a39 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -366,15 +366,24 @@ Body::asVector ( const Matrix<double,STATESIZE,1> &m ) * Description: Returns the current quaternion estimate. *-------------------------------------------------------------------------------------- */ +#if STATESIZE==13 Quaterniond Body::qhat ( ) { -#if STATESIZE==13 Quaterniond qbw(X(9),X(6),X(7),X(8)); -#else - fprintf(stderr, "Quaternion is not being estimated, quitting.\n"); - exit(1); -#endif return qbw; } /* ----- end of method Body::qhat ----- */ +void +Body::qhat ( const Quaterniond &q ) +{ + X[6] = q.x(); + X[7] = q.y(); + X[8] = q.z(); + X[9] = q.w(); + return ; +} /* ----- end of method Body::qhat ----- */ + +#endif + + |