Fixed paths that curve left/right

They previously under or over shot the target yaw