Convergence problems economic NMPC


I am struggling with the convergence of the solvers for my economic NMPC problem.

The problem itself I will say is a bit special formulated but it is reasonable to do so. Also, there is already literature available where similar approaches are done but mostly solved with dynamic programming.

I attached a runnable example and also a detailed description of my OCP formulation at this link:

The short summary of my OCP is:

  • Economic cost function (quadratic and linear terms). → Therefore used the external cost module
  • model derivatives are with respect to speed and not to time. → See the detailed description for more information
  • Only linear constraints.
  • Two constraints are soft constraints.

The example contains also a runable .m file for Matlab and windows. In this example the weired convergence behaviour is shown. I have tried a lot of different solver setting but it is still worst for every setting.

Especially with the SQP_RTI solver I get some strange behaviour. Maybe it is a bug? The solver returns 0 (success) but if I look at the state evolution, the fourth state (time) is negativ which is impossible for only positive speeds.

Also, I figured out the closer I got to zero with the velocity as initial state (less than 5 m/s) the more difficult is it for the solver to converge. Also, when the initial state lies outside of the soft constraint it is really hard for the solver to converge.

Nevertheless, In the attached example the initial state is inside the bounds so in general the OCP should be feasible.

But, to be honest, after all different settings and configurations I tried I didn’t managed my problem to converge and I have really no idea why.

It would be a pleasure for me if someone would spend some time to take a closer look to my example.:slight_smile:

Thanks a lot in advance.

Best regards


Hi Matthias,

I just had a brief look and ran your code.

I suggest you start from something simpler.

Did you try to initialize the solver with a feasible solution?
Did you try to remove constraints?
Did you try the acados integrator separately and check if it works as you expect?
I saw you have some splines in the dynamics.

I don’t think that the economic cost term is the only/main issue. Because it also didn’t work when only using the quadratic term.


Hi Jonathan,

thanks for your answer.

I just made a few steps back to a simpler problem trying to figure out what goes wrong.

Steps I made so far are:

  • Checked the integrator for the proposed model → works fine also for small velocity values (1m/s).
  • Removed the splines and used a 2-dimensional quadratic polynomial approximation instead.
  • Removed all constraints
  • Initialized with a feasible solution
  • Removed the last state of the model which causes a deadtime in the prediction (and is only needed for rate penalization and directly control the torque in the first state with the input.
  • Also removed the second last state (time) in the model.
  • Only used quadratic costs for the dynamics.

What I now figured out is:

  • When I simulate the unconstrained model everything works like expected for higher velocity values (bigger than 1.5). But if I set the initial state constraint and the solver initialization to
v0 = 1;
ocp.set('constr_x0', [v0; 0; 0]);
x_traj_init = [v0*ones(1, NpMpc+1);zeros(n_states-1,NpMpc+1)];

the solver returns also zero (success) with all the prediction steps being NaN. This can be fixed by using a higher initialization value for x_traj_init but then the prediction is wrong.

Also using stabilizing terminal constraints do not solve this.

I attached it as a runnable example and hope your have the time to check this. Maybe this is some solver issue?

Again, thanks a lot for your quick response and help! :slight_smile:

Best regards


Hi Matthias,

I saw that you do full condensing, there can be some problem with the conditioning.
Changing to no condensing, i.e.

qp_solver = 'partial_condensing_hpipm';
qp_solver_cond_N = NpMpc; % for partial condensing

made it work for me.