I am questioning if there is an issue with Step 3 of finding the Cauchy Point.
You are updating the vector 'p' during the loop, but according to the paper,
'p' should remain fixed within a single iteration. I think the correct approach
to updating vector 'p' is to use a temporary vector to record its dynamics and
then add them up outside the loop.