Efficient formulation and solution of nonlinear model predictive control problem