Modified from TimeDepend2, speed up calculation by turning N*N*2*2
matrix into (2*N)*(2*N) matrix (Gr and Sigma)