changing the cost function for distance to observation to return a point3D and note a 1D distance.
Seems to help convergence and computing time. Probably because Jacobian is way easier to compute this way