Autonomous Systems Laboratory, Department of Mechanical Engineering, State University of New York at Stony Brook