New position algorithm, still a bit jerky