Integral action & anti-windup

Improves steady-state tracking while limiting integrator problems when actuators saturate.