https://123dok.net/document/yr366577-newton-feasible-iterates-nonlinear-predictive-control-walking-situation.html