Fix speed calculation (position not working)