The previous implementation of pfold was inccorect when used with non-zero pdot and pddot. Also the implementation seemed unecessary complicated and potentially slow.
