Update motion model with position instead of speed

Use time difference to update motion model