Refactor position update. Fix bugs in position update.