Maybe the sign of Kd or the error term is incorrect.
Also, for the approach to work well, the time interval between calculations must be constant. I see nothing in the incomplete code that you posted which suggests attention to this important detail.
Try another PID library, like this one, which is known to work well.