diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp
index 3c47fb0230..ce3b374992 100644
--- a/src/lib/matrix/matrix/SquareMatrix.hpp
+++ b/src/lib/matrix/matrix/SquareMatrix.hpp
@@ -148,6 +148,21 @@ public:
 		return self.trace<M>(0);
 	}
 
+	// keep the sub covariance matrix and zero all covariance elements related
+	// to the rest of the matrix
+	template <size_t Width>
+	void uncorrelateCovarianceBlock(size_t first)
+	{
+		static_assert(Width <= M, "Width bigger than matrix");
+		assert(first + Width <= M);
+
+		SquareMatrix<Type, M> &self = *this;
+		SquareMatrix<Type, Width> cov = self.slice<Width, Width>(first, first);
+		self.slice<M, Width>(0, first) = 0.f;
+		self.slice<Width, M>(first, 0) = 0.f;
+		self.slice<Width, Width>(first, first) = cov;
+	}
+
 	// zero all offdiagonal elements and keep corresponding diagonal elements
 	template <size_t Width>
 	void uncorrelateCovariance(size_t first)
diff --git a/src/lib/matrix/test/MatrixSquareTest.cpp b/src/lib/matrix/test/MatrixSquareTest.cpp
index da9e7df76c..eafca26d74 100644
--- a/src/lib/matrix/test/MatrixSquareTest.cpp
+++ b/src/lib/matrix/test/MatrixSquareTest.cpp
@@ -118,6 +118,16 @@ TEST(MatrixSquareTest, Square)
 	SquareMatrix<float, 4> E_check(data_E_check);
 	EXPECT_EQ(E, E_check);
 
+	SquareMatrix<float, 4> A_block(data_4x4);
+	A_block.uncorrelateCovarianceBlock<2>(1);
+	float data_A_block_check[16] = {1, 0, 0, 4,
+					0, 6, 7, 0,
+					0, 10, 11, 0,
+					13, 0, 0, 16
+				       };
+	SquareMatrix<float, 4> A_block_check(data_A_block_check);
+	EXPECT_EQ(A_block, A_block_check);
+
 	// test symmetric functions
 	SquareMatrix<float, 4> F(data_4x4);
 	F.makeBlockSymmetric<2>(1);