lots of additional debugging tools implemented to check parts of the algorithm. still no good

include/msckf_vio/math_utils.hpp

@@ -54,10 +54,39 @@ inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q)
   p(1) = -q(1);
   p(2) = -q(2);
   p(3) =  q(3);
-
+  quaternionNormalize(p);
   return p;
 }
 
+/*
+ * @brief converts a vector4 to a vector3, dropping (3)
+ *        this is typically used to get the vector part of a quaternion
+          for eq small angle approximation
+*/
+inline Eigen::Vector3d v4tov3(const Eigen::Vector4d& q)
+{
+  Eigen::Vector3d p;
+  p(0) = q(0);
+  p(1) = q(1);
+  p(2) = q(2);
+  return p;
+}
+
+/*
+ * @brief Perform q1 * q2
+ */
+
+inline Eigen::Vector4d QtoV(const Eigen::Quaterniond& q)
+{
+  Eigen::Vector4d p;
+  p(0) = q.x();
+  p(1) = q.y();
+  p(2) = q.z();
+  p(3) = q.w();
+  return p;
+}
+
+
 /*
  * @brief Perform q1 * q2
  */

include/msckf_vio/msckf_vio.h

@@ -160,11 +160,14 @@ class MsckfVio {
         const CameraMeasurementConstPtr& feature_msg);
 
 
+    void calcErrorState();
+
     // Debug related Functions
     // Propagate the true state
     void batchTruthProcessing(
         const double& time_bound);
 
+
     void processTruthtoIMU(const double& time,
         const Eigen::Vector4d& m_rot,
         const Eigen::Vector3d& m_trans);
@@ -339,6 +342,9 @@ class MsckfVio {
     ros::Publisher mocap_odom_pub;
     geometry_msgs::TransformStamped raw_mocap_odom_msg;
     Eigen::Isometry3d mocap_initial_frame;
+
+    Eigen::Vector4d mocap_initial_orientation;
+    Eigen::Vector3d mocap_initial_position;
 };
 
 typedef MsckfVio::Ptr MsckfVioPtr;