Added initial working inverted pendulum model with constraint optimal control.
Added initial working inverted pendulum model with constraint optimal control.

wuyou33/MPCToolbox,ovidiualexandru/MPCToolbox