Modified pid control to take into account that the real input is U = Ubar + du, where du depend on the reference error and Ubar depend on the trajectory chose. With this modification, at error = 0 correspond an input drag U = Ubar