https://123dok.net/document/yd7kemdg-second-order-analysis-state-constrained-control-affine-problems.html