diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 0b496d2a4..88fb9ba0b 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -118,6 +118,8 @@ void LevenbergMarquardtOptimizer::iterate() {
 
         size_t dim = key_value.value.dim();
         Matrix A = Matrix::Identity(dim, dim);
+        //Replace the identity matrix with diagonal of Hessian
+        A.diagonal()=diagHessian.at(key_value.key);
         Vector b = Vector::Zero(dim);
         SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
         dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);