Update matrix_utils.hpp