ARC Centre of Excellence for Autonomous Systems, School of Mechanical and Manufacturing Engineering, University of New South Wales