Hi Hossein,
When using 'fmincon' to solve an optimal control problem for a nonlinear system, one common issue is that the optimization doesn't converge because the initial guess is too far from a feasible point. It helps if the initial guess already roughly satisfies the constraints (even though 'fmincon' can technically handle infeasible starting points, having a feasible or near-feasible initial guess usually makes a big difference)
Another thing you might want to check is the choice of algorithm. By default, 'fmincon' might not pick the best one for your type of problem. You can set an appropriate algorithm for the task by adding setting the 'Algorithm' when creating the options. https://www.mathworks.com/help/optim/ug/fmincon.html#busog7r-options.
It can also be useful to play with the solver tolerances. Sometimes the default tolerances are either too tight or too loose for a particular problem.
Lastly, if the solver is still having a hard time finding a feasible solution, enabling the feasibility mode is worth trying. It tells fmincon to first try to find any feasible point before worrying about optimizing the cost. This can be turnen on with 'EnableFeasibilityMode', true in the options. You can refer to the documentation for more information: https://www.mathworks.com/help/optim/ug/constrained-nonlinear-optimization-algorithms.html#mw_e825c521-552e-4f75-80e8-8a5c2d2062ee
Hope this helps.
