PK fixed the problem of large error by switching to ode45 since the system seems to no longer be stiff.

He also made the controller able to track a constant reference.