diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp
index 6d51e4f..466d1e9 100644
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -498,15 +498,9 @@ double Feature::cvKernel(
   double delta = 0;
 
   if(type == "Sobel_x")
-  {
-    std::cout << "image value x" << ((double)xderImage.at<short>(pose.y, pose.x))/255. << std::endl;
     delta = ((double)xderImage.at<short>(pose.y, pose.x))/255.;
-  }
   else if (type == "Sobel_y")
-  {
-    std::cout << "image value y" << ((double)yderImage.at<short>(pose.y, pose.x))/255. << std::endl;
     delta = ((double)yderImage.at<short>(pose.y, pose.x))/255.;
-  }
   return delta;
 
 }
diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 0fc464a..f6602c1 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -21,10 +21,10 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="3"/>
+      <param name="patch_size_n" value="5"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 0877890..977a95b 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -212,7 +212,7 @@ bool MsckfVio::loadParameters() {
     utils::getTransformEigen(nh, "T_imu_body").inverse();
 
   // Maximum number of camera states to be stored
-  nh.param<int>("max_cam_state_size", max_cam_state_size, 30);
+  nh.param<int>("max_cam_state_size", max_cam_state_size, 20);
   //cam_state_size = max_cam_state_size;
   ROS_INFO("===========================================");
   ROS_INFO("fixed frame id: %s", fixed_frame_id.c_str());
@@ -1320,16 +1320,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
     // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame);
     // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame);
     
-    dI_dhj(0, 0) = dx;// /(pixelDistance.x);
-    dI_dhj(0, 1) = dy;// /(pixelDistance.y);
-
-    gradientVector.x += dx;
-    gradientVector.y += dy; 
-    
-
-    residualVector.x += dx * r_photo(count);
-    residualVector.y += dy * r_photo(count);
-    res_sum += r_photo(count);
+    dI_dhj(0, 0) = dx * cam0.intrinsics[0];
+    dI_dhj(0, 1) = dy * cam0.intrinsics[1];
 
     //dh / d{}^Cp_{ij}
     dh_dCpij(0, 0) = 1 / p_c0(2);
@@ -1424,7 +1416,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   {  
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
     feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
-    feature.VisualizeKernel(cam_state, cam_state_id, cam0);
+    //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
   return;
@@ -1767,7 +1759,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
   // Compute the error of the state.
   VectorXd delta_x = K * r;
-
   // Update the IMU state.
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
@@ -1823,6 +1814,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 }
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
+  return 1;
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
 
@@ -2085,7 +2077,7 @@ void MsckfVio::pruneLastCamStateBuffer()
     else
       featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
 
-    if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
+    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
@@ -2232,7 +2224,7 @@ void MsckfVio::pruneCamStateBuffer() {
     else
       featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
 
-    if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
+    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();