diff options
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 + + |