https://github.com/aleksandarhaber/model-predictive-control-for-linear-systems-in-cpp-by-using-eigen-library