diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index 1b5ff40497096ac74f35024ce0b37b1c88076266..6975a798db580e1c127b0215609f49904b29ae40 100644
--- a/hello_wolf/factor_bearing.h
+++ b/hello_wolf/factor_bearing.h
@@ -69,8 +69,8 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
     T bearing   = atan2(point_r(1), point_r(0));
 
     // 4. Get the measured range-and-bearing to the point, and its covariance
-    Matrix<T, 2, 1> range_bearing       = getMeasurement().cast<T>();
-    Matrix<T, 2, 2> range_bearing_cov   = getMeasurementCovariance().cast<T>();
+    Matrix<T, 2, 1> range_bearing       = getMeasurement();
+    Matrix<T, 2, 2> range_bearing_cov   = getMeasurementCovariance();
 
     // 5. Get the bearing error by comparing against the bearing measurement
     T er   = range_bearing(1) - bearing;
diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h
index 0ca3ec0a4f31a403424f928b38cc98015ea728d7..436b79e4a0eee67aa8ddd97de34ec14127251d01 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/hello_wolf/factor_range_bearing.h
@@ -115,7 +115,7 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
     exp_rb(1)      = atan2(lmk_s(1), lmk_s(0));        // bearing
 
     // 4. Get the measured range-and-bearing to the point
-    Matrix<T, 2, 1> meas_rb       = getMeasurement().cast<T>(); // cast Eigen type vector to have scalar type 'T'
+    auto& meas_rb = getMeasurement();
 
     // 5. Get the error by comparing the expected against the measurement
     Matrix<T, 2, 1> err_rb        = meas_rb - exp_rb;
@@ -131,7 +131,7 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
     //    R^T * R = Omega
     // in other words, R is the Cholesky decomposition of Omega.
     // NOTE: you get R directly from the Feature with getMeasurementSquareRootInformationUpper()
-    res     = getMeasurementSquareRootInformationUpper().cast<T>() * err_rb;
+    res     = getMeasurementSquareRootInformationUpper() * err_rb;
 
     return true;
 }
diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h
index 7c5c2d3767cef907138b8d60fdbc9090a3a71158..624112000fe6f019e6d86fc9e310e4da9a4a2b3f 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/include/core/factor/factor_autodiff_distance_3D.h
@@ -60,10 +60,12 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
                 norm_squared += (T)1e-8;
             }
             Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
-            Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
-            Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();
+            // Matrix<T,1,1> dist_meas (getMeasurement().cast<T>());
+            // Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper();
 
-            res  = sqrt_info_upper * (dist_meas - dist_exp);
+            // res  = sqrt_info_upper * (dist_meas - dist_exp);
+
+            res  = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp);
 
             return true;
         }
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 5f6e149883ff1499757af2db05a9f8fc2a956b16..2338f699de73d38c74ce13509cdc0b70a1a82130 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -110,26 +110,19 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
 
     Eigen::Map<const Eigen::Matrix<T, INTRINSICS_SIZE, 1> > c(_c);
 
-    // Compute corrected delta
 
-    /// Is this my delta_preint ? Yes!
-    const Eigen::Matrix<T, 3, 1> delta_preint = getMeasurement().cast<T>();
+    // Correct delta due to changes in the calibration parameters
+    auto delta_corrected = getMeasurement() + J_delta_calib_ * (c - calib_preint_);
 
-    Matrix<T, 3, 1> c_preint = calib_preint_.cast<T>();
-
-    Eigen::Matrix<T, 3, 1> delta_corrected = delta_preint + J_delta_calib_.cast<T>() * (c - c_preint);
-
-    // 2d pose residual
 
+    // Predict delta from states
     Eigen::Matrix<T, 3, 1> delta_predicted;
-
     // position
     delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2 - p1);
-
     // orientation
     delta_predicted(2) = pi2pi(_o2[0] - _o1[0]);
 
-    // residual
+    // 2d pose residual
     residuals = delta_corrected - delta_predicted;
 
     // angle remapping
@@ -139,7 +132,7 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
         residuals(2) = residuals(2) + T(2. * M_PI);
 
     // weighted residual
-    residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
+    residuals = getMeasurementSquareRootInformationUpper() * residuals;
 
     return true;
 }
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index 2f1a011c19ae11ae78aaea9a17fd1c5b0d23fe5c..64d09368e97d0f5e394672de4218d7e608c49445 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2D.h
@@ -71,14 +71,14 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
     expected_measurement(2) = o2 - o1;
 
     // Error
-    er = expected_measurement - getMeasurement().cast<T>();
+    er = expected_measurement - getMeasurement();
     while (er(2) > T( M_PI ))
         er(2) -=   T( 2 * M_PI );
     while (er(2) < T( -M_PI ))
         er(2) +=   T( 2 * M_PI );
 
     // Residuals
-    res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
+    res = getMeasurementSquareRootInformationUpper() * er;
 
     ////////////////////////////////////////////////////////
     // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h
index d359adbbd275a3faca43d1a154590830cd9425a6..1b53915402fe5adb6a1f1481fefc06add006c291 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_odom_3D.h
@@ -214,7 +214,8 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
     expectation(_p_current, _q_current, _p_past, _q_past, expected.data(), expected.data()+3);
 
     // measured motion increment, dp_m, dq_m
-    Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
+    // Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
+    auto& dp_m = getMeasurement().head<3>();
     Eigen::Quaternion<T> dq_m(getMeasurement().tail<4>().cast<T>());
 
     Eigen::Matrix<T,3,1> dp = expected.head(3);
@@ -227,7 +228,7 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
     residuals.head(3) = dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
     residuals.tail(3) = q2v(dq.conjugate() * dq_m);
 
-    residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
+    residuals = getMeasurementSquareRootInformationUpper() * residuals;
 
     return true;
 }
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
index 91229e4c83614df3a92d03ff9e24a407a719eadb..fece6f0bc06d0352319ceaa30e614200a6d254e9 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/include/core/factor/factor_pose_2D.h
@@ -47,7 +47,8 @@ template<typename T>
 inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
 {
     // measurement
-    Eigen::Matrix<T,3,1> meas =  getMeasurement().cast<T>();
+    // Eigen::Matrix<T,3,1> meas =  getMeasurement().cast<T>();
+    auto& meas =  getMeasurement();
 
     // error
     Eigen::Matrix<T,3,1> er;
@@ -61,7 +62,7 @@ inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _
 
     // residual
     Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
-    res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+    res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
 
     ////////////////////////////////////////////////////////
     // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
index 81b1837969a3ef62aa069aece3864713d6b31e76..c82a1788f6919fd66844e3790c616c6afd54b7b8 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/include/core/factor/factor_pose_3D.h
@@ -56,12 +56,12 @@ inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _
 
     // error
     Eigen::Matrix<T, 6, 1> er;
-    er.head(3)        = p_measured.cast<T>() - p;
+    er.head(3)        = p_measured - p;
     er.tail(3)        = q2v(q.conjugate() * q_measured.cast<T>());
 
     // residual
     Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
-    res               = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+    res               = getFeature()->getMeasurementSquareRootInformationUpper() * er;
 
     return true;
 }
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 4c8c7bf1a735b76b9ac3cbcb12d246a728fd806a..6533bd414d54a9eb45da8c92336ba3d1f0e07739 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -78,7 +78,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
 
     // residual
     Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
-    res               = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+    res               = getFeature()->getMeasurementSquareRootInformationUpper() * er;
 
     return true;
 }