diff --git a/include/msckf_vio/math_utils.hpp b/include/msckf_vio/math_utils.hpp
index 29fb540..d6cb57a 100644
--- a/include/msckf_vio/math_utils.hpp
+++ b/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
  */
diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h
index 27b0ef5..bcf5ee2 100644
--- a/include/msckf_vio/msckf_vio.h
+++ b/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;
diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch
index 7b3b45f..c4e12e7 100644
--- a/launch/msckf_vio_debug_tum.launch
+++ b/launch/msckf_vio_debug_tum.launch
@@ -22,8 +22,8 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="true"/>
-      <param name="GroundTruth" value="true"/>
+      <param name="PrintImages" value="false"/>
+      <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="7"/>
 
diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index eca1ebc..38f93a2 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -22,7 +22,7 @@
 
       <!-- Debugging Flaggs -->
       <param name="PrintImages" value="true"/>
-      <param name="GroundTruth" value="false"/>
+      <param name="GroundTruth" value="true"/>
 
       <param name="patch_size_n" value="7"/>
       <!-- Calibration parameters -->
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 8289cb3..2473996 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -203,6 +203,9 @@ bool MsckfVio::loadParameters() {
 
   state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
   state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
+  true_state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
+  true_state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
+
   CAMState::T_cam0_cam1 =
     utils::getTransformEigen(nh, "cam1/T_cn_cnm1");
   IMUState::T_imu_body =
@@ -345,6 +348,54 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){
 }
 
 void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){
+  static bool first_truth_odom_msg = true;
+
+  // errorstate
+  /*if(not ErrorState)
+    return;
+  */
+
+  // If this is the first mocap odometry messsage, set
+  // the initial frame.
+  if (first_truth_odom_msg) {
+    Quaterniond orientation;
+    Vector3d translation;
+    tf::vectorMsgToEigen(
+        msg->transform.translation, translation);
+    tf::quaternionMsgToEigen(
+        msg->transform.rotation, orientation);
+    
+    mocap_initial_orientation = QtoV(orientation);
+    mocap_initial_position = translation;
+
+    first_truth_odom_msg = false;
+  }
+
+  // Transform the ground truth.
+  Quaterniond orientation;
+  Vector3d translation;
+  //tf::vectorMsgToEigen(
+  //    msg->transform.translation, translation);
+  //tf::quaternionMsgToEigen(
+  //    msg->transform.rotation, orientation);
+  tf::vectorMsgToEigen(
+      msg->transform.translation, translation);
+  tf::quaternionMsgToEigen(
+      msg->transform.rotation, orientation);
+
+  Eigen::Vector4d q = quaternionMultiplication(quaternionConjugate(mocap_initial_orientation), QtoV(orientation));
+
+  translation -= mocap_initial_position;
+
+  msg->transform.rotation.x = q(0);
+  msg->transform.rotation.y = q(1);
+  msg->transform.rotation.z = q(2);
+  msg->transform.rotation.w = q(3);
+
+  msg->transform.translation.x = translation(0);
+  msg->transform.translation.y = translation(1);
+  msg->transform.translation.z = translation(2);
+
   truth_msg_buffer.push_back(*msg);
 }
 
@@ -376,16 +427,32 @@ void MsckfVio::imageCallback(
   batchImuProcessing(feature_msg->header.stamp.toSec());
   
   // save true state in seperate state vector
-  if(GROUNDTRUTH)
+  
+  //if(ErrState)
+  //{
     batchTruthProcessing(feature_msg->header.stamp.toSec());
+    
+    if(GROUNDTRUTH)
+    {
+      state_server.imu_state.position = true_state_server.imu_state.position;
+      state_server.imu_state.orientation = true_state_server.imu_state.orientation;
+      state_server.imu_state.position_null = true_state_server.imu_state.position_null;
+      state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null;
 
+      state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0;
+      state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu;
+    }
+  //}
   double imu_processing_time = (
       ros::Time::now()-start_time).toSec();
 
   // Augment the state vector.
   start_time = ros::Time::now();
   if(PHOTOMETRIC)
+  {
+    truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
     PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
+  }
   else
     stateAugmentation(feature_msg->header.stamp.toSec());
   double state_augmentation_time = (
@@ -673,15 +740,47 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
 
 void MsckfVio::calcErrorState()
 {
-  // true_state_server - state_server = err_state_server
-  StateServer errState;
-  errState.imu_state.id = state_server.imu_state.id;
-  errState.imu_state.time = state-server.imu_state.time;
 
-  errState.imu_state.orientation = quaternionMultiplication(true_state_server.imu_state.orientation, quaterionConjugate(state_server.imu_state.orientation));
-  errState.imu_state.position = true_state_server.imu_state.position - state_server.imu_state.position;
-  errState.imu_state.velocity = 
+  // imu error
+  err_state_server.imu_state.id = state_server.imu_state.id;
+  err_state_server.imu_state.time = state_server.imu_state.time;
 
+  err_state_server.imu_state.orientation = quaternionMultiplication(true_state_server.imu_state.orientation, quaternionConjugate(state_server.imu_state.orientation));
+  
+  // convert to small angle approximation
+  err_state_server.imu_state.orientation *= 2;
+  err_state_server.imu_state.orientation(3) = 0;
+
+  err_state_server.imu_state.position = true_state_server.imu_state.position - state_server.imu_state.position;
+  err_state_server.imu_state.velocity = true_state_server.imu_state.velocity - state_server.imu_state.velocity;
+
+  err_state_server.imu_state.gyro_bias = true_state_server.imu_state.gyro_bias - true_state_server.imu_state.gyro_bias;
+  err_state_server.imu_state.gyro_bias = true_state_server.imu_state.acc_bias - true_state_server.imu_state.acc_bias; 
+
+  err_state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0 - true_state_server.imu_state.R_imu_cam0; 
+  err_state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu - true_state_server.imu_state.t_cam0_imu; 
+
+  err_state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null - true_state_server.imu_state.orientation_null; 
+  err_state_server.imu_state.position_null = true_state_server.imu_state.position_null - true_state_server.imu_state.position_null; 
+  err_state_server.imu_state.velocity_null = true_state_server.imu_state.velocity_null - true_state_server.imu_state.velocity_null; 
+
+  auto cam_state_iter = state_server.cam_states.begin();
+  auto true_cam_state_iter = true_state_server.cam_states.begin();
+  auto err_cam_state_iter = err_state_server.cam_states.begin();
+  
+  for (int i = 0; i < state_server.cam_states.size(); ++i, ++cam_state_iter, ++err_cam_state_iter, ++true_cam_state_iter) 
+  {
+    // calculate error in camera rotation
+    Eigen::Vector4d q = cam_state_iter->second.orientation;
+    Eigen::Vector4d p = quaternionConjugate(true_cam_state_iter->second.orientation);
+    err_cam_state_iter->second.orientation = quaternionMultiplication(p, q);
+    // small angle approximation
+    err_cam_state_iter->second.orientation *= 2;
+    err_cam_state_iter->second.orientation(3) = 0;
+
+    // calculate error of state position
+    err_cam_state_iter->second.position = true_cam_state_iter->second.position - cam_state_iter->second.position; 
+  }
 }
 
 void MsckfVio::batchTruthProcessing(const double& time_bound) {
@@ -706,7 +805,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
     m_rotation[1] = truth_msg.transform.rotation.y;
     m_rotation[2] = truth_msg.transform.rotation.z;
     m_rotation[3] = truth_msg.transform.rotation.w;
-    
+    quaternionNormalize(m_rotation);
     // Execute process model.
     processTruthtoIMU(truth_time, m_rotation, m_translation);
     ++used_truth_msg_cntr;
@@ -714,8 +813,8 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
   last_time_bound = time_bound;
 
   // Set the state ID for the new IMU state.
-  true_state_server.imu_state.id = IMUState::next_id++;
-
+  true_state_server.imu_state.id = IMUState::next_id;
+  err_state_server.imu_state.id = IMUState::next_id;
   // Remove all used Truth msgs.
   truth_msg_buffer.erase(truth_msg_buffer.begin(),
       truth_msg_buffer.begin()+used_truth_msg_cntr);
@@ -960,7 +1059,6 @@ void MsckfVio::stateAugmentation(const double& time)
   size_t old_rows = state_server.state_cov.rows();
   size_t old_cols = state_server.state_cov.cols();
 
-  // add 7 for camera state + irradiance bias eta = b_l
   state_server.state_cov.conservativeResize(old_rows+6, old_cols+6);
 
   // Rename some matrix blocks for convenience.
@@ -991,18 +1089,26 @@ void MsckfVio::truePhotometricStateAugmentation(const double& time)
   // Add a new camera state to the state server.
   Matrix3d true_R_w_i = quaternionToRotation(
       true_state_server.imu_state.orientation);
-  Matrix3d true_R_w_c = R_i_c * R_w_i;
+  Matrix3d true_R_w_c = true_R_i_c * true_R_w_i;
   Vector3d true_t_c_w = true_state_server.imu_state.position +
-    R_w_i.transpose()*t_c_i;
+    true_R_w_i.transpose()*true_t_c_i;
 
   true_state_server.cam_states[true_state_server.imu_state.id] =
-    CAMState(state_server.imu_state.id);
+    CAMState(true_state_server.imu_state.id);
   CAMState& true_cam_state = true_state_server.cam_states[
     true_state_server.imu_state.id];
 
+  // manage error state size
+  err_state_server.cam_states[err_state_server.imu_state.id] =
+    CAMState(err_state_server.imu_state.id);
+  CAMState& err_cam_state = err_state_server.cam_states[
+    err_state_server.imu_state.id];
+
+  err_cam_state.time = time;
+
   true_cam_state.time = time;
   true_cam_state.orientation = rotationToQuaternion(true_R_w_c);
-  true_cam_state.position = t_c_w;
+  true_cam_state.position = true_t_c_w;
 
   true_cam_state.orientation_null = true_cam_state.orientation;
   true_cam_state.position_null = true_cam_state.position;
@@ -1163,6 +1269,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   int count = 0;
   double dx, dy;
+
   for (auto point : feature.anchorPatch_3d)
   {
     Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
@@ -1197,6 +1304,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
     //d{}^Gp_P{ij} / \rho_i
     double rho = feature.anchor_rho;
+    // Isometry T_anchor_w takes a vector in anchor frame to world frame
     dGpj_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho));
 
     dGpj_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear()
@@ -1216,7 +1324,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
     count++;
   }
-
   // calculate residual
 
   //observation
@@ -1299,7 +1406,26 @@ void MsckfVio::PhotometricFeatureJacobian(
     std::cout << "stopped playpack" << std::endl;
     nh.setParam("/play_bag", false);
   }
+
+
+  // Errstate
+  calcErrorState();
+  /*
+  std::cout << "--- error state: ---\n " << std::endl;
+  std::cout << "imu orientation:\n " << err_state_server.imu_state.orientation << std::endl;
+  std::cout << "imu position\n" << err_state_server.imu_state.position << std::endl;
+  
+  int count = 0;
+  for(auto cam_state : err_state_server.cam_states)
+  {
+    std::cout << " - cam " << count++ << " - \n" << std::endl;
+    std::cout << "orientation: " << cam_state.second.orientation(0) << cam_state.second.orientation(1) << " " << cam_state.second.orientation(2) << " " << std::endl;
+    std::cout << "position:" << cam_state.second.position(0) << " " << cam_state.second.position(1) << " " << cam_state.second.position(2) << std::endl;
+  }
+  */
+
   const auto& feature = map_server[feature_id];
+  
 
   // Check how many camera states in the provided camera
   // id camera has actually seen this feature.
@@ -1344,18 +1470,24 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   // get Nullspace
   JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
+  
   int sv_size = 0;
   Eigen::VectorXd singularValues = svd_helper.singularValues();
   for(int i = 0; i < singularValues.size(); i++)
-    if(singularValues[i] > 1e-5)
+    if(singularValues[i] > 1e-9)
       sv_size++;
 
-  int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size();
-  MatrixXd A = svd_helper.matrixU().rightCols(
-      jacobian_row_size-sv_size);
+  int null_space_size = svd_helper.matrixU().cols() - sv_size; //TEST used instead of svd_helper.singularValues().size();
+  MatrixXd A = svd_helper.matrixU().rightCols(null_space_size);
   
   H_x = A.transpose() * H_xi;
   r = A.transpose() * r_i;
+  
+  ofstream myfile;
+  myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
+  myfile << "nulls:\n" << A.transpose() * H_yi <<endl;
+  myfile.close();
+  cout << "---------- LOGGED -------- " << endl;
 
   if(PRINTIMAGES)
   {
@@ -1495,6 +1627,13 @@ void MsckfVio::featureJacobian(
    H_x = A.transpose() * H_xj;
    r = A.transpose() * r_j;
 
+  ofstream myfile;
+  myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
+  myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
+  myfile.close();
+  cout << "---------- LOGGED -------- " << endl;
+  nh.setParam("/play_bag", false);
+
   return;
 }
 
@@ -1926,9 +2065,18 @@ void MsckfVio::pruneCamStateBuffer() {
     }
 
     // Remove this camera state in the state vector.
-    state_server.cam_states.erase(cam_id);
-    cam0.moving_window.erase(cam_id);
-    cam1.moving_window.erase(cam_id);
+
+
+    for (const auto& cam_id : rm_cam_state_ids) {
+
+      state_server.cam_states.erase(cam_id);
+      cam0.moving_window.erase(cam_id);
+      cam1.moving_window.erase(cam_id);
+      
+      // Remove this camera state in the true state vector.
+      true_state_server.cam_states.erase(cam_id);
+      err_state_server.cam_states.erase(cam_id);
+    }
   }
 
   return;
@@ -2084,7 +2232,6 @@ void MsckfVio::publish(const ros::Time& time) {
   feature_msg_ptr->width = feature_msg_ptr->points.size();
 
   feature_pub.publish(feature_msg_ptr);
-
   return;
 }