diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 465a7883b..de77dbc47 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -99,7 +99,7 @@ void LevenbergMarquardtOptimizer::iterate() {
     cout << "linearizing = " << endl;
   GaussianFactorGraph::shared_ptr linear = linearize();
 
-  double modelFidelity =  std::numeric_limits<size_t>::max();
+  double modelFidelity = 0.0;
 
   // Keep increasing lambda until we make make progress
   while (true) {
@@ -158,23 +158,20 @@ void LevenbergMarquardtOptimizer::iterate() {
 
       // cost change in the original, possibly nonlinear system (old - new)
       double costChange = state_.error - error;
-      std::cout <<  "costChange " << costChange << std::endl;
 
       // cost change in the linearized system (old - new)
-      std::cout <<  "graph_ " << graph_.size() << std::endl;
-      std::cout <<  "linear " << linear->size() << std::endl;
-      linear->print("linear");
-      std::cout <<  "linear->error(delta) " << linear->error(delta) << std::endl;
       double linearizedCostChange = state_.error - linear->error(delta);
-      std::cout <<  "linearizedCostChange " << linearizedCostChange << std::endl;
 
+      // checking similarity between change in original and linearized cost
       modelFidelity = costChange / linearizedCostChange;
-      std::cout <<  "modelFidelity " << modelFidelity << std::endl;
 
-      if (error <= state_.error) {
+      if (error < state_.error) {
         state_.values.swap(newValues);
         state_.error = error;
-        decreaseLambda(modelFidelity);
+        if(modelFidelity > params_.minModelFidelity)
+          decreaseLambda(modelFidelity);
+        else
+          increaseLambda(modelFidelity);
         break;
       } else {
         // Either we're not cautious, or the same lambda was worse than the current error.
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
index ddaa8f09c..a3c9b14bd 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
@@ -50,11 +50,12 @@ public:
   double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0)
   VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity
   bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed
+  double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration
   std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda]
 
   LevenbergMarquardtParams() :
       lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(0.0),
-      verbosityLM(SILENT), disableInnerIterations(false) {
+      verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(1e-3) {
   }
   virtual ~LevenbergMarquardtParams() {
   }