Add integral filter abstraction

CMakeLists.txt

@@ -65,6 +65,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
 )
@@ -112,6 +113,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
   )

doc/integral_tustin.ipynb

@@ -0,0 +1,130 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# How to discretize a integral filter with Tustin's method\n",
+    "First a continous time filter is constructed. This filter will be discretized with Tustin's method and converted into C++ code."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from sympy import *"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "s, T, z = symbols('s,T,z')"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "First our continous time system"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "sys = 1 / s"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Translate to discrete"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/latex": [
+       "$\\displaystyle \\frac{T \\left(z + 1\\right)}{2 \\left(z - 1\\right)}$"
+      ],
+      "text/plain": [
+       "T*(z + 1)/(2*(z - 1))"
+      ]
+     },
+     "execution_count": 4,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "sys = sys.subs(s, 2 / T * (z - 1) / (z + 1))\n",
+    "sys"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Translate that to C++"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "```\n",
+    "H = y/u = T / 2 * (z + 1)/(z - 1)\n",
+    "\n",
+    "u * T / 2 * (z + 1) = y * (z - 1)\n",
+    "u * T / 2 * (1 + z^-1) = y * (1 - z^-1)\n",
+    "T / 2 * (u[0] + u[1]) = y[0] - y[1]\n",
+    "y[0] = T / 2 * (u[0] + u[1]) + y[1]\n",
+    "```\n",
+    "\n",
+    "```c++\n",
+    "y[0] = T / 2 * (u[0] + u[1]) + y[1]\n",
+    "```"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.8.10"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}

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

@@ -427,26 +427,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 *****/
@@ -728,8 +724,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;
@@ -923,6 +919,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();
@@ -975,11 +973,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,28 @@
+#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
+
+  // A continous time integrator was discretized with Tustin's method. For a mathematical
+  // explanation, see doc/integral_tustin.ipynb
+  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);
+  }
+}