I did what you suggested and get that infeasible result. I still don't know why, theoretically it has been proved that this algorithm is recursively feasible if starting from a feasible solution and if you have a feasible control action to append at the end of the next warm start. If you see the for the first p iteration, the three subproblems are feasible, then a convex combination of their solutions should be also feasible for the same time instant k but in p+1.
If you noticed in line 67, I have equality constraints (besides the terminal equality constraint), if I check the following sum given a feasible solution it should be equal to 0 due to the constraint. For the controller with the zero objective I get
K>> S.E*Cu*sol{2}{1}(:,k)+S.Ed*S.d(k,:)'
ans =
-1.0595e-14
1.7764e-15
6.3665e-12
5.6843e-14
0
0
0
-3.4106e-13
0
-2.8422e-14
0
and for the same constraint but with the real controller (the feasible solution in the round before the error) I get
K>> S.E*Cu*sol{2}{1}(:,k)+S.Ed*S.d(k,:)'
ans =
-1.0595e-14
1.7764e-15
0
-3.6794e-07
0
0
1.6266e-05
-3.4106e-13
0
-2.8422e-14
-7.7511e-07
then, when I do the combination of all the solutions of the real controller, the same equality constraint that should sum 0, gives
ans =
-3.4401e-15
0
1.1369e-13
-1.2142e-07
7.9012e-12
2.0373e-10
5.3679e-06
7.5666e-08
1.2733e-11
1.5153e-07
-6.3459e-07
Given the order of the results, I don't know if this satisfy the exact equality constraints at p=2, even when theory says that the signal Us (line 167) should remain feasible.
Other thing I checked was the terminal constraint. If I take the difference between the terminal state given by the solution and the target state it is 0. But if I compute the terminal state by applying manually the sequence of inputs of the solution, then the difference is not 0 but an approximate value.
Could be a numerical issue due to the equality constraints?.