traj iterative solver tweaked. Still not quite as accurate as I'd like it. Probe following wrong solution as well.