SYSTEMS AND METHODS FOR MANEUVERING VEHICLES USING PREDICTIVE CONTROL AND AUTOMATED DRIVING

System, methods, and other embodiments described herein relate to vehicle maneuvering using predictive control with automated driving and contingency planning to preserve safety and increase comfort. In one embodiment, a method includes receiving, by a controller, an operator command associated with a vehicle maneuver while automated driving is engaged. The method also includes adjusting a motion command associated with the vehicle maneuver by applying a predictive control according to motion estimates outputted from the automated driving and the operator command, the predictive control using motion constraints that are constant between time intervals for the vehicle maneuver. The method also includes controlling, by the controller, a vehicle using the motion command for a time step during the time intervals.