System for controlling an internal combustion engine having a variable geometry turbocharger, an EGR valve and an EGR throttle. The system includes a controller and an engine. In each sampling period, the controller inputs sensor values and estimators that are calculated based on the sensor values and dynamic models and records the sensor values and the estimators in a memory of the controller. The controller incorporates the sensor values and the estimators into conditions for optimality associated with a constrained optimal control problem, maps the conditions for optimality to a non-smooth system using Fischer-Burmeister function, performs Newton method iterations on a smoothed system approximating the non-smooth system in order to converge on a solution, and issues commands that control the EGR valve, the EGR throttle and the variable geometry turbocharger during engine operation.