diff options
| -rw-r--r-- | src/body.cpp | 27 | ||||
| -rw-r--r-- | src/body.h | 1 |
2 files changed, 22 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 ----- */ + @@ -37,6 +37,7 @@ class Body Matrix<double,1,13> H(); Matrix<double,13,13> Q(double dt); Matrix<double,1,1> S(const Matrix<double,13,13> &Pxx); + Quaterniond qhat(); #else Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt); Matrix<double,9,1> asVector(); |