With MATLAB built-in solver
There is a new m-file: demo2.m, where now I use MATLAB built-in solver. Thus, the simulation becomes faster. Please see problem_formalization.pdf to understand the formalization of the problem such that it can be put into MATLAB built-in solver.