Full state mostly implemented.

STATESIZE 9 works just as it does for experiment04032017 tag. The
quaternion covariance is not being set correctly yet.

1 files changed, 24 insertions, 1 deletions

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