diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp
index c5daecd9d..24bc9b1cf 100644
--- a/cpp/LinearFactor.cpp
+++ b/cpp/LinearFactor.cpp
@@ -167,17 +167,20 @@ void LinearFactor::tally_separator(const string& key, set<string>& separator) co
 /* ************************************************************************* */  
 pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
 
-  // get pointers to the matrices
-  vector<const Matrix *> matrices;
-  BOOST_FOREACH(string j, ordering) {
-    const Matrix& Aj = get_A(j);
-    matrices.push_back(&Aj);
-  }
+	// get pointers to the matrices
+	vector<const Matrix *> matrices;
+	BOOST_FOREACH(string j, ordering) {
+		const Matrix& Aj = get_A(j);
+		matrices.push_back(&Aj);
+	}
 
-  // divide in sigma so error is indeed 0.5*|Ax-b|
-  Matrix t = diag(ediv(ones(sigmas_.size()),sigmas_));
-  Matrix A = t*collect(matrices);
-  return make_pair(A, t*b_);
+	// divide in sigma so error is indeed 0.5*|Ax-b|
+	Vector t = ediv(ones(sigmas_.size()),sigmas_);
+	Matrix A = vector_scale(collect(matrices), t);
+	Vector b(b_);
+	for (int i=0; i<b_.size(); ++i)
+		b(i) *= t(i);
+	return make_pair(A, b);
 }
 
 /* ************************************************************************* */
@@ -196,8 +199,8 @@ Matrix LinearFactor::matrix_augmented(const Ordering& ordering) const {
 	matrices.push_back(&B_mat);
 
 	// divide in sigma so error is indeed 0.5*|Ax-b|
-	Matrix t = diag(ediv(ones(sigmas_.size()),sigmas_));
-	Matrix A = t*collect(matrices);
+	Vector t = ediv(ones(sigmas_.size()),sigmas_);
+	Matrix A = vector_scale(collect(matrices), t);
 	return A;
 }
 
diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index b95bda78c..acb681751 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -427,6 +427,28 @@ Matrix collect(size_t nrMatrices, ...)
 return collect(matrices);
 }
 
+/* ************************************************************************* */
+// row scaling
+Matrix vector_scale(const Matrix& A, const Vector& v) {
+	Matrix M(A);
+	for (int i=0; i<A.size1(); ++i) {
+		for (int j=0; j<A.size2(); ++j) {
+			M(i,j) *= v(i);
+		}
+	}
+	return M;
+}
+
+/* ************************************************************************* */
+// column scaling
+Matrix vector_scale(const Vector& v, const Matrix& A) {
+	Matrix M(A);
+	for (int i=0; i<A.size1(); ++i)
+		for (int j=0; j<A.size2(); ++j)
+			M(i,j) *= v(j);
+	return M;
+}
+
 /* ************************************************************************* */
 Matrix skewSymmetric(double wx, double wy, double wz)
 {
diff --git a/cpp/Matrix.h b/cpp/Matrix.h
index 577558834..cc562c15a 100644
--- a/cpp/Matrix.h
+++ b/cpp/Matrix.h
@@ -186,6 +186,14 @@ Matrix stack(size_t nrMatrices, ...);
 Matrix collect(std::vector<const Matrix *> matrices);
 Matrix collect(size_t nrMatrices, ...);
 
+/**
+ * scales a matrix row or column by the values in a vector
+ * Arguments (Matrix, Vector) scales the rows,
+ * (Vector, Matrix) scales the columns
+ */
+Matrix vector_scale(const Matrix& A, const Vector& v); // row
+Matrix vector_scale(const Vector& v, const Matrix& A); // column
+
 /**
  * skew symmetric matrix returns this:
  *   0  -wz   wy
diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp
index aa3c50432..0874a76d4 100644
--- a/cpp/testMatrix.cpp
+++ b/cpp/testMatrix.cpp
@@ -114,6 +114,46 @@ TEST( matrix, zeros )
   EQUALITY(A , zero);
 }
 
+/* ************************************************************************* */
+TEST( matrix, scale_columns )
+{
+	Matrix A(3,4);
+	A(0,0) = 1.; A(0,1) = 1.; A(0,2)= 1.; A(0,3)= 1.;
+	A(1,0) = 1.; A(1,1) = 1.; A(1,2)= 1.; A(1,3)= 1.;
+	A(2,0) = 1.; A(2,1) = 1.; A(2,2)= 1.; A(2,3)= 1.;
+
+	Vector v = Vector_(4, 2., 3., 4., 5.);
+
+	Matrix actual = vector_scale(v,A);
+
+	Matrix expected(3,4);
+	expected(0,0) = 2.; expected(0,1) = 3.; expected(0,2)= 4.; expected(0,3)= 5.;
+	expected(1,0) = 2.; expected(1,1) = 3.; expected(1,2)= 4.; expected(1,3)= 5.;
+	expected(2,0) = 2.; expected(2,1) = 3.; expected(2,2)= 4.; expected(2,3)= 5.;
+
+	CHECK(assert_equal(actual, expected));
+}
+
+/* ************************************************************************* */
+TEST( matrix, scale_rows )
+{
+	Matrix A(3,4);
+	A(0,0) = 1.; A(0,1) = 1.; A(0,2)= 1.; A(0,3)= 1.;
+	A(1,0) = 1.; A(1,1) = 1.; A(1,2)= 1.; A(1,3)= 1.;
+	A(2,0) = 1.; A(2,1) = 1.; A(2,2)= 1.; A(2,3)= 1.;
+
+	Vector v = Vector_(3, 2., 3., 4.);
+
+	Matrix actual = vector_scale(A,v);
+
+	Matrix expected(3,4);
+	expected(0,0) = 2.; expected(0,1) = 2.; expected(0,2)= 2.; expected(0,3)= 2.;
+	expected(1,0) = 3.; expected(1,1) = 3.; expected(1,2)= 3.; expected(1,3)= 3.;
+	expected(2,0) = 4.; expected(2,1) = 4.; expected(2,2)= 4.; expected(2,3)= 4.;
+
+	CHECK(assert_equal(actual, expected));
+}
+
 /* ************************************************************************* */
 TEST( matrix, equal )
 {