Add quaternion accessor to Body.

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  ----- */
+
diff --git a/src/body.h b/src/body.h
index b973b00..5544bba 100644
--- a/src/body.h
+++ b/src/body.h
@@ -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();