Fixed G matrix implementation loop and initialization of the matrix.