Fix #351, fixed coordinates interpolation algorithm wrong behavior (#352)

Now the coordinates between the first and the second timestamp in the queue get interpolated correctly instead of just returning the first set of coordinates without performing the interpolation.