Fix kp / kd