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 aab2ecf8f081e..edad56b460093 100644
--- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -1053,6 +1053,8 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS
double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data)
{
// NOTE: Acceleration command is always positive even if the ego drives backward.
+ const double nearest_target_vel =
+ control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps;
const double vel_sign = (control_data.shift == Shift::Forward)
? 1.0
: (control_data.shift == Shift::Reverse ? -1.0 : 0.0);
@@ -1076,8 +1078,8 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
const double error_vel_filtered = m_lpf_vel_error->filter(diff_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(
+ diff_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.
@@ -1087,7 +1089,9 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
constexpr double ff_scale_max = 2.0; // for safety
constexpr double ff_scale_min = 0.5; // for safety
const double ff_scale = std::clamp(
- std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max);
+ 1.0 + (abs(nearest_target_vel - current_vel) / std::max(abs(current_vel), 0.1)), ff_scale_min,
+ ff_scale_max);
+
const double ff_acc =
control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale;
diff --git a/control/pid_longitudinal_controller/test/test_pid.cpp b/control/pid_longitudinal_controller/test/test_pid.cpp
index 82d01e0028a9c..f0ceccd8499a3 100644
--- a/control/pid_longitudinal_controller/test/test_pid.cpp
+++ b/control/pid_longitudinal_controller/test/test_pid.cpp
@@ -29,14 +29,14 @@ TEST(TestPID, calculate_pid_output)
PIDController pid;
// Cannot calculate before initializing gains and limits
- EXPECT_THROW(pid.calculate(0.0, dt, enable_integration, contributions), std::runtime_error);
+ EXPECT_THROW(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), std::runtime_error);
pid.setGains(1.0, 1.0, 1.0);
pid.setLimits(10.0, 0.0, 10.0, 0.0, 10.0, 0.0, 10.0, 0.0);
double error = target - current;
double prev_error = error;
while (current != target) {
- current = pid.calculate(error, dt, enable_integration, contributions);
+ current = pid.calculate(error, error, dt, enable_integration, contributions);
EXPECT_EQ(contributions[0], error);
EXPECT_EQ(contributions[1], 0.0); // integration is deactivated
EXPECT_EQ(contributions[2], error - prev_error);
@@ -50,18 +50,18 @@ TEST(TestPID, calculate_pid_output)
enable_integration = true;
// High errors to force each component to its upper limit
- EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0);
+ EXPECT_EQ(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), 0.0);
for (double error = 100.0; error < 1000.0; error += 100.0) {
- EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), 10.0);
+ EXPECT_EQ(pid.calculate(error, error, dt, enable_integration, contributions), 10.0);
EXPECT_EQ(contributions[0], 10.0);
EXPECT_EQ(contributions[1], 10.0); // integration is activated
EXPECT_EQ(contributions[2], 10.0);
}
// Low errors to force each component to its lower limit
- EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0);
+ EXPECT_EQ(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), 0.0);
for (double error = -100.0; error > -1000.0; error -= 100.0) {
- EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), -10.0);
+ EXPECT_EQ(pid.calculate(error, error, dt, enable_integration, contributions), -10.0);
EXPECT_EQ(contributions[0], -10.0);
EXPECT_EQ(contributions[1], -10.0); // integration is activated
EXPECT_EQ(contributions[2], -10.0);