New tracking algo & servo fixes