Imported from https://github.com/obastani/model-predictive-shielding