https://9lib.org/document/zkwx2e1z-optimal-control-adaptive-receding-horizon-discrete-piecewise-systems.html