Add integral filter abstraction

CMakeLists.txt

@@ -66,6 +66,7 @@ add_library(${PROJECT_NAME}
   src/controller.cpp
   src/calculations.cpp
   src/details/derivative.cpp
+  src/details/integral.cpp
   src/details/second_order_lowpass.cpp
   src/visualization.cpp
 )
@@ -113,6 +114,7 @@ if(CATKIN_ENABLE_TESTING)
     test/unittests/test_calculations.cpp
     test/unittests/test_derivative.cpp
     test/unittests/test_fifo_array.cpp
+    test/unittests/test_integral.cpp
     test/unittests/test_main.cpp
     test/unittests/test_second_order_lowpass.cpp
   )

include/path_tracking_pid/controller.hpp

@@ -11,6 +11,7 @@
 #include <boost/noncopyable.hpp>
 #include <path_tracking_pid/details/derivative.hpp>
 #include <path_tracking_pid/details/fifo_array.hpp>
+#include <path_tracking_pid/details/integral.hpp>
 #include <path_tracking_pid/details/second_order_lowpass.hpp>
 #include <vector>
 
@@ -35,14 +36,14 @@ struct ControllerState
   double previous_steering_yaw_vel = 0.0;
   bool end_phase_enabled = false;
   bool end_reached = false;
-  double error_integral_lat = 0.0;
-  double error_integral_ang = 0.0;
   double tracking_error_lat = 0.0;
   double tracking_error_ang = 0.0;
   // Errors with little history
   details::SecondOrderLowpass error_lat;
-  details::Derivative error_deriv_lat;
   details::SecondOrderLowpass error_ang;
+  details::Integral error_integral_lat;
+  details::Integral error_integral_ang;
+  details::Derivative error_deriv_lat;
   details::Derivative error_deriv_ang;
 };
 

include/path_tracking_pid/details/integral.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <path_tracking_pid/details/fifo_array.hpp>
+
+namespace path_tracking_pid::details
+{
+/**
+ * @brief Discrete time integral filter
+ */
+class Integral
+{
+  constexpr static auto NaN = std::numeric_limits<double>::quiet_NaN();
+
+public:
+  Integral() = default;
+
+  /**
+   * @brief Construct an integral filter
+   * @param windup_limit Integral windup limit
+   */
+  explicit Integral(double windup_limit);
+
+  /**
+   * @brief Change the parameters of the filter
+   * @param windup_limit Integral windup limit
+   */
+  void configure(double windup_limit);
+
+  /**
+   * @brief Filter one sample of a signal
+   * @param u Signal to be filtered
+   * @param step_size Time step from previous sample
+   * @return Integral of the signal
+   */
+  double filter(double u, double step_size);
+
+  /**
+   * @brief Reset the signal buffers
+   */
+  void reset();
+
+private:
+  FifoArray<double, 2> u_ = {};
+  FifoArray<double, 2> y_ = {};
+  double windup_limit_ = NaN;
+};
+
+}  // namespace path_tracking_pid::details

src/controller.cpp

@@ -473,26 +473,22 @@ Controller::UpdateResult Controller::update(
   dt.toSec();
 
   // integrate the error
-  controller_state_.error_integral_lat += error_lat_filtered * dt.toSec();
-  controller_state_.error_integral_ang += error_ang_filtered * dt.toSec();
-
-  // Apply windup limit to limit the size of the integral term
-  controller_state_.error_integral_lat =
-    std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit);
-  controller_state_.error_integral_ang =
-    std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit);
+  auto error_integral_lat =
+    controller_state_.error_integral_lat.filter(error_lat_filtered, dt.toSec());
+  auto error_integral_ang =
+    controller_state_.error_integral_ang.filter(error_lat_filtered, dt.toSec());
 
   // Take derivative of error, first the raw unfiltered data:
   auto error_deriv_lat = controller_state_.error_deriv_lat.filter(error_lat_filtered, dt.toSec());
   auto error_deriv_ang = controller_state_.error_deriv_ang.filter(error_ang_filtered, dt.toSec());
 
   // calculate the control effort
   const auto proportional_lat = config_.Kp_lat * error_lat_filtered;
-  const auto integral_lat = config_.Ki_lat * controller_state_.error_integral_lat;
+  const auto integral_lat = config_.Ki_lat * error_integral_lat;
   const auto derivative_lat = config_.Kd_lat * error_deriv_lat;
 
   const auto proportional_ang = config_.Kp_ang * error_ang_filtered;
-  const auto integral_ang = config_.Ki_ang * controller_state_.error_integral_ang;
+  const auto integral_ang = config_.Ki_ang * error_integral_ang;
   const auto derivative_ang = config_.Kd_ang * error_deriv_ang;
 
   /***** Compute forward velocity *****/
@@ -775,8 +771,8 @@ Controller::UpdateResult Controller::update(
   // Publish control effort if controller enabled
   if (!enabled_)  // Do nothing reset integral action and all filters
   {
-    controller_state_.error_integral_lat = 0.0;
-    controller_state_.error_integral_ang = 0.0;
+    controller_state_.error_integral_lat.reset();
+    controller_state_.error_integral_ang.reset();
   }
 
   controller_state_.current_x_vel = new_x_vel;
@@ -970,6 +966,8 @@ void Controller::configure(path_tracking_pid::PidConfig & config)
 {
   controller_state_.error_lat.configure(config.lowpass_cutoff, config.lowpass_damping);
   controller_state_.error_ang.configure(config.lowpass_cutoff, config.lowpass_damping);
+  controller_state_.error_integral_lat.configure(windup_limit);
+  controller_state_.error_integral_ang.configure(windup_limit);
 
   // Erase all queues when config changes
   controller_state_.error_lat.reset();
@@ -1022,11 +1020,11 @@ void Controller::reset()
   controller_state_.previous_steering_angle = 0.0;
   controller_state_.previous_steering_yaw_vel = 0.0;
   controller_state_.previous_steering_x_vel = 0.0;
-  controller_state_.error_integral_lat = 0.0;
-  controller_state_.error_integral_ang = 0.0;
   controller_state_.error_lat.reset();
-  controller_state_.error_deriv_lat.reset();
   controller_state_.error_ang.reset();
+  controller_state_.error_integral_lat.reset();
+  controller_state_.error_integral_ang.reset();
+  controller_state_.error_deriv_lat.reset();
   controller_state_.error_deriv_ang.reset();
 }
 

src/details/integral.cpp

@@ -0,0 +1,26 @@
+#include <path_tracking_pid/details/integral.hpp>
+
+namespace path_tracking_pid::details
+{
+Integral::Integral(double windup_limit) : windup_limit_(windup_limit) {}
+
+void Integral::configure(double windup_limit) { windup_limit_ = windup_limit; }
+
+double Integral::filter(double u, double step_size)
+{
+  // save history
+  u_.push(u);
+  y_.push(u);  // increase index so the math below looks correct
+
+  auto T = step_size;
+  y_[0] = T / 2 * (u_[0] + u_[1]) + y_[1];
+  y_[0] = std::clamp(y_[0], -windup_limit_, windup_limit_);
+  return y_[0];
+}
+
+void Integral::reset()
+{
+  u_ = {};
+  y_ = {};
+}
+}  // namespace path_tracking_pid::details

test/unittests/test_integral.cpp

@@ -0,0 +1,24 @@
+#include <gtest/gtest.h>
+
+#include <path_tracking_pid/details/integral.hpp>
+#include <vector>
+
+using path_tracking_pid::details::Integral;
+
+constexpr double eps = 1e-6;
+
+TEST(Integral, StepResponse)
+{
+  double dt = 0.1;
+  double windup_limit = 0.5;
+
+  Integral filter{windup_limit};
+
+  std::vector<double> expected_response = {0.05, 0.15, 0.25, 0.35, 0.45, 0.5, 0.5};
+  for (int i = 0; i < static_cast<int>(expected_response.size()); ++i) {
+    SCOPED_TRACE(i);
+
+    auto result = filter.filter(1, dt);
+    EXPECT_NEAR(result, expected_response[i], eps);
+  }
+}