From 2c0ce68acd031b67fdffaea3b2e50cca4f487633 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 4 Apr 2017 16:10:26 -0500
Subject: Full state mostly implemented.

STATESIZE 9 works just as it does for experiment04032017 tag. The
quaternion covariance is not being set correctly yet.
---
 src/body.cpp | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

(limited to 'src/body.cpp')

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
+
+
-- 
cgit v1.1