diff --git a/src/thermostat/pid_state.rs b/src/thermostat/pid_state.rs
index 5319b3c..99c41a0 100644
--- a/src/thermostat/pid_state.rs
+++ b/src/thermostat/pid_state.rs
@@ -96,7 +96,6 @@ impl PidState {
     //       + x0 * (kp + ki + kd)
     //       - x1 * (kp + 2kd)
     //       + x2 * kd
-    //       + kp * (u0 - u1)
     // y0  = clip(y0', ymin, ymax)
     pub fn update_pid(&mut self) -> Option<f64> {
         let input = self.get_temperature()?.get::<degree_celsius>();
@@ -107,8 +106,7 @@ impl PidState {
                     self.controller.parameters.kp + self.controller.parameters.ki + self.controller.parameters.kd,
                 )
             - self.controller.x1 * f64::from(self.controller.parameters.kp + 2.0 * self.controller.parameters.kd)
-            + self.controller.x2 * f64::from(self.controller.parameters.kd)
-            + f64::from(self.controller.parameters.kp) * (setpoint as f64 - self.controller.u1);
+            + self.controller.x2 * f64::from(self.controller.parameters.kd);
         if output < self.controller.parameters.output_min.into() {
             output = self.controller.parameters.output_min.into();
         }