The emerging application of autonomous driving provides the benefit of eliminating the driver from the control loop, which offers opportunities for safety, energy saving and green house gas emissions reduction by adjusting the speed trajectory. The technological advances in sensing and computing make it realistic for the vehicle to obtain a preview information of its surrounding environment, and optimize its speed trajectory accordingly using predictive planning methods. Conventional speed following algorithms usually adopt an energy-centric perspective and improve fuel economy by means of reducing the power loss due to braking and operating the engine at its high fuel efficiency region. This could be a problem for diesel-powered vehicles, ...