diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp
index b035e86b3..3db5baa32 100644
--- a/cpp/testGaussianFactorGraph.cpp
+++ b/cpp/testGaussianFactorGraph.cpp
@@ -691,7 +691,7 @@ TEST( GaussianFactorGraph, elimination )
 	GaussianFactorGraph fg;
 	Matrix Ap = eye(1), An = eye(1) * -1;
 	Vector b = Vector_(1, 0.0);
-  SharedDiagonal sigma = sharedSigma(2,2.0);
+  SharedDiagonal sigma = sharedSigma(1,2.0);
 	fg.add("x1", An, "x2", Ap, b, sigma);
 	fg.add("x1", Ap, b, sigma);
 	fg.add("x2", Ap, b, sigma);
diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp
index 6a88be8a2..429b2f6ee 100644
--- a/cpp/testMatrix.cpp
+++ b/cpp/testMatrix.cpp
@@ -170,9 +170,12 @@ TEST( matrix, insert_column )
 
 	insertColumn(big, col, j);
 
-	Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
-			0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-			1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0);
+	Matrix expected = Matrix_(5, 6,
+			0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 1.0, 0.0, 0.0);
 
 	CHECK(assert_equal(expected, big));
 }
@@ -181,15 +184,21 @@ TEST( matrix, insert_column )
 TEST( matrix, insert_subcolumn )
 {
 	Matrix big = zeros(5, 6);
-	Vector col = ones(2);
+	Vector col1 = ones(2);
 	size_t i = 1;
 	size_t j = 3;
 
-	insertColumn(big, col, i, j);
+	insertColumn(big, col1, i, j); // check 1
 
-	Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-			0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+	Vector col2 = ones(1);
+	insertColumn(big, col2, 4, 5); // check 2
+
+	Matrix expected = Matrix_(5, 6,
+			0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
 
 	CHECK(assert_equal(expected, big));
 }