diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 274c226ed779ced623f5ae8c0eb76c0e5bc4d291..d26cfcd7c996f10ae7b81bdb4a7eb406a876fbf2 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -104,26 +104,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
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 6caa5f868adceb291690590455bea94520771b0f..d210d941b516b0651fb6f871bbdcc6a48b1ec69f 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -620,7 +620,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 
 
     WOLF_TRACE("\n  ========== SOLVE =========");
-    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
     WOLF_TRACE("\n", report);
 
     WOLF_TRACE("x1           : ", problem->getState(N*dt).transpose());