Changed the phi from double to vector and resized it to the correct number of dimensions in the for loop. Appears to be working fine.
phi has dimension p+1 - p from the dimensionality and + 1 for the correlation parameters for the Zs.