Optimization-based Fault Mitigation in Automated Driving