agrees with PFClusterAlgo to 1e-5 tolerance.

Had to place numerical stability requirements
in both new and old pieces of algoritms to
regularize the behavior of the position fit.