https://github.com/aleksandarhaber/linear-quadratic-regulator-optimal-control-in-cpp-from-scratch-by-using-newton-method