I have problems introducing a slack variable to soften some constraints I have on the states. I use QUADPROG to solve the problem which is written in the form
S.t Aeq*x = beq
lb <= x <=ub
The vector x contains 3 states z1, z2 and z3 as well as the the calculated control input u. For instance, for prediction horizon 1 the vector will have 4 rows, for prediction horizon 2 it will grow to 8 etc. Now I have constraints on the input varible which I put in the vectors lb and ub. Everything works fine until I introduce constraints on the states. These are hard inequality constraints. As expected the problem becomes infeasible so I would need to relax them. Therein lies the issue, I do not know how to modify the setup in order to incorporate a slack variable.
I understand that the state vector has to be expanded by adding an extra variable s => 0 preferably in the end. Consequently I have to increase the size of H by 1, so the last diagonal element will be the weight on the slack varible which of course should be heavily penalized. Now if I would like to soften the constraint on the first state z1 I need to modify the first entry in ub and lb by adding the slack variable s.
So far I have only modified H as mentioned above but I do not know how to continue with the vectors ub and lb, which I assume also have to be modified in some way.
Thank you in advance for any advice!