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/state.cpp | 25 ++++++++++++++++++++++++-
 1 file changed, 24 insertions(+), 1 deletion(-)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index dfe41fa..e53ad29 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -464,7 +464,7 @@ void
 #if STATESIZE==13
 State::addFeatures ( std::vector<measurement_t> &F, double z)
 #else
-State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z);
+State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z)
 #endif
 {
 #if STATESIZE==13
@@ -1008,3 +1008,26 @@ State::asVector ( const Matrix<double,Dynamic,1> &m )
     return ;
 }		/* -----  end of method State::asVector  ----- */
 
+#if STATESIZE==13
+Quaterniond
+State::qhat ( )
+{
+    return body->qhat() ;
+}		/* -----  end of method State::qhat  ----- */
+
+void
+State::qhat ( const Quaterniond &q )
+{
+    body->qhat(q);
+    return ;
+}		/* -----  end of method State::qhat  ----- */
+
+
+void
+State::quaternion_covariance (  )
+{
+    P.block<4,4>(6,6) = 1e-5*Matrix<double,4,4>::Identity();
+    return ;
+}		/* -----  end of method State::quaternion_covariance  ----- */
+
+#endif
-- 
cgit v1.1