diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp
index 5f61b3d1a..ec709b64b 100644
--- a/cpp/Vector.cpp
+++ b/cpp/Vector.cpp
@@ -238,6 +238,13 @@ namespace gtsam {
   	return result;
   }
 
+  /* ************************************************************************* */
+  void scal(double alpha, Vector& x) {
+  	size_t n = x.size();
+  	for (size_t i = 0; i < n; i++)
+  		x[i] *= alpha;
+  }
+
   /* ************************************************************************* */
   void axpy(double alpha, const Vector& x, Vector& y) {
   	size_t n = x.size();
diff --git a/cpp/Vector.h b/cpp/Vector.h
index ef575035c..9dd746975 100644
--- a/cpp/Vector.h
+++ b/cpp/Vector.h
@@ -201,6 +201,11 @@ double max(const Vector &a);
 /** Dot product */
 double dot(const Vector &a, const Vector& b);
 
+/**
+ * BLAS Level 1 scal: x <- alpha*x
+ */
+void scal(double alpha, Vector& x);
+
 /**
  * BLAS Level 1 axpy: y <- alpha*x + y
  */
diff --git a/cpp/VectorConfig.cpp b/cpp/VectorConfig.cpp
index 839756259..338026fa8 100644
--- a/cpp/VectorConfig.cpp
+++ b/cpp/VectorConfig.cpp
@@ -192,6 +192,12 @@ double dot(const VectorConfig& a, const VectorConfig& b) {
 	return a.dot(b);
 }
 
+/* ************************************************************************* */
+void scal(double alpha, VectorConfig& x) {
+	for (VectorConfig::iterator xj = x.begin(); xj != x.end(); xj++)
+		scal(alpha, xj->second);
+}
+
 /* ************************************************************************* */
 void axpy(double alpha, const VectorConfig& x, VectorConfig& y) {
 	VectorConfig::const_iterator xj = x.begin();
diff --git a/cpp/VectorConfig.h b/cpp/VectorConfig.h
index ab84a1c84..d6f6f9ff1 100644
--- a/cpp/VectorConfig.h
+++ b/cpp/VectorConfig.h
@@ -137,6 +137,11 @@ namespace gtsam {
   /** Dot product */
   double dot(const VectorConfig&, const VectorConfig&);
 
+  /**
+   * BLAS Level 1 scal: x <- alpha*x
+   */
+  void scal(double alpha, VectorConfig& x);
+
   /**
    * BLAS Level 1 axpy: y <- alpha*x + y
    * UNSAFE !!!! Only works if x and y laid out in exactly same shape
diff --git a/cpp/iterative-inl.h b/cpp/iterative-inl.h
index 05b8e7c75..d993c9b19 100644
--- a/cpp/iterative-inl.h
+++ b/cpp/iterative-inl.h
@@ -42,17 +42,17 @@ namespace gtsam {
 
 			// calculate optimal step-size
 			E Ad = Ab * d;
-			double alpha = -dot(d, g) / dot(Ad, Ad);
+			double alpha = - dot(d, g) / dot(Ad, Ad);
 
 			// do step in new search direction
-			x += alpha * d;
+			axpy(alpha, d, x); // x += alpha*d
 			if (k==maxIterations) break;
 
 			// update gradient (or re-calculate at reset time)
 			if (k%reset==0)
 				g = Ab.gradient(x);
 			else
-				axpy(alpha, Ab ^ Ad, g);
+				axpy(alpha, Ab ^ Ad, g);  // g += alpha*(Ab^Ad)
 
 			// check for convergence
 			double dotg = dot(g, g);
@@ -66,7 +66,9 @@ namespace gtsam {
 			else {
 				double beta = dotg / prev_dotg;
 				prev_dotg = dotg;
-				d = g + d*beta;
+				// d = g + d*beta;
+				scal(beta,d);
+				axpy(1.0, g, d);
 			}
 		}
 		return x;
diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorConfig.cpp
index 3036a8cbf..43d06cd70 100644
--- a/cpp/testVectorConfig.cpp
+++ b/cpp/testVectorConfig.cpp
@@ -132,6 +132,17 @@ TEST( VectorConfig, axpy) {
   CHECK(assert_equal(expected,y));
 }
 
+/* ************************************************************************* */
+TEST( VectorConfig, scal) {
+  VectorConfig x,expected;
+  x += VectorConfig("x",Vector_(3, 1.0, 2.0, 3.0));
+  x += VectorConfig("y",Vector_(2, 4.0, 5.0));
+  expected += VectorConfig("x",Vector_(3, 10.0, 20.0, 30.0));
+  expected += VectorConfig("y",Vector_(2, 40.0, 50.0));
+  scal(10,x);
+  CHECK(assert_equal(expected,x));
+}
+
 /* ************************************************************************* */
 TEST( VectorConfig, update_with_large_delta) {
 	// this test ensures that if the update for delta is larger than