diff --git a/firmware/heater_control.cpp b/firmware/heater_control.cpp
index 6558870..0884d0d 100644
--- a/firmware/heater_control.cpp
+++ b/firmware/heater_control.cpp
@@ -78,7 +78,7 @@ static HeaterState GetNextState(HeaterState state, float sensorEsr)
     return state;
 }
 
-static Pid heaterPid(0.003f, 0.005f, 0.6f, HEATER_CONTROL_PERIOD);
+static Pid heaterPid(0.003f, 0.005f, 0, 0.6f, HEATER_CONTROL_PERIOD);
 
 static float GetDutyForState(HeaterState state, float heaterEsr)
 {
diff --git a/firmware/pid.cpp b/firmware/pid.cpp
index a8cede9..42dfc67 100644
--- a/firmware/pid.cpp
+++ b/firmware/pid.cpp
@@ -7,10 +7,15 @@ float Pid::GetOutput(float setpoint, float observation)
     // Integrate error
     m_integrator += error * m_period * m_ki;
 
+    // Differentiate error
+    float errorDelta = error - m_lastError;
+    float dEdt = errorDelta / m_period;
+    m_lastError = error;
+
     // Clamp to +- 1
     if (m_integrator > m_clamp) m_integrator = m_clamp;
     if (m_integrator < -m_clamp) m_integrator = -m_clamp;
 
     // Multiply by gains and sum
-    return m_kp * error + m_integrator;
+    return m_kp * error + m_integrator + m_kd * dEdt;
 }
diff --git a/firmware/pid.h b/firmware/pid.h
index 3636cba..3109c33 100644
--- a/firmware/pid.h
+++ b/firmware/pid.h
@@ -3,10 +3,11 @@
 class Pid
 {
 public:
-    Pid(float kP, float kI, float clamp, int periodMs) 
+    Pid(float kP, float kI, float kD, float clamp, int periodMs) 
         : m_period(periodMs / 1000.0f)
         , m_kp(kP)
         , m_ki(kI)
+        , m_kd(kD)
         , m_clamp(clamp)
     {
     }
@@ -17,6 +18,7 @@ private:
     const float m_period;
     const float m_kp;
     const float m_ki;
+    const float m_kd;
     const float m_clamp;
 
     float m_lastError;
diff --git a/firmware/pump_control.cpp b/firmware/pump_control.cpp
index c67f321..9193e29 100644
--- a/firmware/pump_control.cpp
+++ b/firmware/pump_control.cpp
@@ -8,7 +8,7 @@
 #include "ch.h"
 
 // Bosch CJ125 is somewhere VERY ROUGHLY like 200-400A/(v*s) integrator gain
-static Pid pumpPid(50.0f, 10000.0f, 10, 2);
+static Pid pumpPid(50.0f, 10000.0f, 0, 10, 2);
 
 static THD_WORKING_AREA(waPumpThread, 256);
 static void PumpThread(void*)