diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 27 |
1 files changed, 21 insertions, 6 deletions
diff --git a/src/body.cpp b/src/body.cpp index 99ea1ed..f7440b5 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -203,11 +203,11 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt ) #ifdef FULLSTATE Matrix<double,13,13> F = Matrix<double,13,13>::Zero(); double qbw1,qbw2,qbw3,qbw4; - qbw1 = X(6); - qbw2 = X(7); - qbw3 = X(8); - qbw4 = X(9); - Quaterniond qbw(qbw4,qbw1,qbw2,qbw3); + Quaterniond qbw = qhat(); + qbw1 = qbw.x(); + qbw2 = qbw.y(); + qbw3 = qbw.z(); + qbw4 = qbw.w(); Matrix<double,3,3> Rbw(qbw.toRotationMatrix()); #else Matrix<double,9,9> F = Matrix<double,9,9>::Zero(); @@ -291,7 +291,7 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond b = Matrix<double,6,1>::Zero(); #ifdef FULLSTATE - Quaterniond qbw(X(9),X(6),X(7),X(8)); + Quaterniond qbw qhat(); Matrix<double,3,3> Rbw(qbw.toRotationMatrix()); #else Matrix<double,3,3> Rbw(q.toRotationMatrix()); @@ -382,3 +382,18 @@ Body::asVector ( const Matrix<double,9,1> &m ) return ; } /* ----- end of method Body::asVector ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: qhat + * Description: Returns the current quaternion estimate. + *-------------------------------------------------------------------------------------- + */ +Quaterniond +Body::qhat ( ) +{ + Quaterniond qbw(X(9),X(6),X(7),X(8)); + return qbw; +} /* ----- end of method Body::qhat ----- */ + |