https://5dok.net/document/yev397x1-penalty-method-based-approach-autonomous-navigation-nonlinear-predictive.html