Formation:

After forming Rho[it+1] and g[it+1] in the main iteration loop, add code to your main program to compute the matrix G and the vector u. A suitable code fragment (not the most efficient, but quite workable) for computing the components of G is

for (itp=1; itp<=it+1; itp++)
  for (itpp=1; itpp<=it+1; itpp++) {
    G[itp][itpp]=0.; /* Trust no one */
    for (k=0; k<=N; k++)
      G[itp][itpp]+=g[itp][k]*g[itpp][k];
  }

Note that you will also have to set the final column G[..][it+2]=-0.5, the final row G[it+2][..]=1, and the bottom corner element G[it+2][it+2]=0 to complete the matrix. Finally, be sure to properly set the values of u[..] as described in the algorithm above.