https://1library.org/document/z127egdy-model-predictive-control-approach-auvs-motion-coordination.html