Changed derivative term to only use input signal (Set Point independant)