ARC Centre of Excellence for Autonomous Systems School of Computer Science and Engineering, University of New South Wales