https://123dok.net/document/y6e4g87z-consistent-approximations-for-the-optimal-control-of-constrained-switched-systems-part-2-an-implementable-algorithm.html