Update motion model with position instead of speed

Use time difference to update motion model
Merge pull request #5 from gbledt/master

Update motion model with position instead of speed