From 0c2c8ef9ba44c5834e2bac0d9946bc1370c63c39 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 4 Mar 2022 15:49:48 +0100 Subject: [PATCH] Add integral filter abstraction --- CMakeLists.txt | 2 + include/path_tracking_pid/controller.hpp | 7 +-- .../path_tracking_pid/details/integral.hpp | 48 +++++++++++++++++++ src/controller.cpp | 28 +++++------ src/details/integral.cpp | 26 ++++++++++ test/unittests/test_integral.cpp | 24 ++++++++++ 6 files changed, 117 insertions(+), 18 deletions(-) create mode 100644 include/path_tracking_pid/details/integral.hpp create mode 100644 src/details/integral.cpp create mode 100644 test/unittests/test_integral.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2833e88f..2ba7f994 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 16cda5a8..acdd571b 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -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; }; diff --git a/include/path_tracking_pid/details/integral.hpp b/include/path_tracking_pid/details/integral.hpp new file mode 100644 index 00000000..cc0e6bf4 --- /dev/null +++ b/include/path_tracking_pid/details/integral.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include + +namespace path_tracking_pid::details +{ +/** + * @brief Discrete time integral filter + */ +class Integral +{ + constexpr static auto NaN = std::numeric_limits::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 u_ = {}; + FifoArray y_ = {}; + double windup_limit_ = NaN; +}; + +} // namespace path_tracking_pid::details diff --git a/src/controller.cpp b/src/controller.cpp index fe6bc822..4d69a1bc 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -473,14 +473,10 @@ 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()); @@ -488,11 +484,11 @@ Controller::UpdateResult Controller::update( // 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(); } diff --git a/src/details/integral.cpp b/src/details/integral.cpp new file mode 100644 index 00000000..7ae9bb80 --- /dev/null +++ b/src/details/integral.cpp @@ -0,0 +1,26 @@ +#include + +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 diff --git a/test/unittests/test_integral.cpp b/test/unittests/test_integral.cpp new file mode 100644 index 00000000..1510fcc7 --- /dev/null +++ b/test/unittests/test_integral.cpp @@ -0,0 +1,24 @@ +#include + +#include +#include + +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 expected_response = {0.05, 0.15, 0.25, 0.35, 0.45, 0.5, 0.5}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +}