diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp
index 8b981c0485ed9..2e989321e0c70 100644
--- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp
+++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp
@@ -29,6 +29,7 @@ class PIDController
   /**
    * @brief calculate the output of this PID
    * @param [in] error previous error
+   * @param [in] error_filtered filtered error
    * @param [in] dt time step [s]
    * @param [in] is_integrated if true, will use the integral component for calculation
    * @param [out] pid_contributions values of the proportional, integral, and derivative components
@@ -36,7 +37,7 @@ class PIDController
    * @throw std::runtime_error if gains or limits have not been set
    */
   double calculate(
-    const double error, const double dt, const bool is_integrated,
+    const double error, const double error_filtered, const double dt, const bool is_integrated,
     std::vector<double> & pid_contributions);
   /**
    * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms
diff --git a/control/pid_longitudinal_controller/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp
index 530b5cd15e754..4ca8ebbd35ef2 100644
--- a/control/pid_longitudinal_controller/src/pid.cpp
+++ b/control/pid_longitudinal_controller/src/pid.cpp
@@ -32,7 +32,7 @@ PIDController::PIDController()
 }
 
 double PIDController::calculate(
-  const double error, const double dt, const bool enable_integration,
+  const double error, const double filtered_error, const double dt, const bool enable_integration,
   std::vector<double> & pid_contributions)
 {
   if (!m_is_gains_set || !m_is_limits_set) {
@@ -55,12 +55,12 @@ double PIDController::calculate(
     error_differential = 0;
     m_is_first_time = false;
   } else {
-    error_differential = (error - m_prev_error) / dt;
+    error_differential = (filtered_error - m_prev_error) / dt;
   }
   double ret_d = p.kd * error_differential;
   ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d);
 
-  m_prev_error = error;
+  m_prev_error = filtered_error;
 
   pid_contributions.resize(3);
   pid_contributions.at(0) = ret_p;
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
index ac968b4e9164c..94ad3c648b571 100644
--- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -977,11 +977,11 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
   const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
   const bool enable_integration =
     (vel_after_delay_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
-  const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - vel_after_delay_abs);
-
+  const double error_vel = abs(target_vel_abs - vel_after_delay_abs);
+  const double error_vel_filtered = m_lpf_vel_error->filter(error_vel);
   std::vector<double> pid_contributions(3);
-  const double pid_acc =
-    m_pid_vel.calculate(error_vel_filtered, control_data.dt, enable_integration, pid_contributions);
+  const double pid_acc = m_pid_vel.calculate(
+    error_vel, error_vel_filtered, control_data.dt, enable_integration, pid_contributions);
 
   // Feedforward scaling:
   // This is for the coordinate conversion where feedforward is applied, from Time to Arclength.