Fault Detection and Isolation for Scan and Plan Robot