https://123dok.net/document/q2nvonjq-distance-estimates-to-feasible-controls-for-systems-with-final-point-constraints-and-second-order-necessary-optimality-conditions.html