GetMatrixIdentity added and tests