https://123dok.net/document/oy80lowq-shooting-algorithm-optimal-control-problems-state-constraints.html