Update motion model with position instead of speed

Use time difference to update motion model instead of propagating forward 1 second. Makes output estimates more smooth.