Fix lowpass & refactor filter structure

- Fixed second order lowpass
- Made the crossover frequency and damping tuneble
- Use the filtered signal for PID calculations
- Add a derivative filter

CMakeLists.txt

@@ -65,8 +65,9 @@ add_library(${PROJECT_NAME}
   src/${PROJECT_NAME}_local_planner.cpp
   src/controller.cpp
   src/calculations.cpp
-  src/visualization.cpp
+  src/details/derivative.cpp
   src/details/second_order_lowpass.cpp
+  src/visualization.cpp
 )
 add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
@@ -109,8 +110,10 @@ install(
 if(CATKIN_ENABLE_TESTING)
   add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false)
   catkin_add_gtest(unittests
-    test/unittests/test_main.cpp
+    test/unittests/test_calculations.cpp
     test/unittests/test_fifo_array.cpp
-    test/unittests/test_calculations.cpp)
+    test/unittests/test_main.cpp
+    test/unittests/test_second_order_lowpass.cpp
+  )
   target_link_libraries(unittests ${catkin_LIBRARIES} ${PROJECT_NAME})
 endif()

cfg/Pid.cfg

@@ -2,6 +2,7 @@
 PACKAGE = "path_tracking_pid"
 
 from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t
+from math import sqrt
 
 gen = ParameterGenerator()
 
@@ -33,6 +34,9 @@ gen.add("Kp_ang", double_t, 0, "Kp Angular", 1, 0, 10)
 gen.add("Ki_ang", double_t, 0, "Ki Angular", 0, 0, 2)
 gen.add("Kd_ang", double_t, 0, "Kd Angular", 0.3, 0, 10)
 
+gen.add("lowpass_cutoff", double_t, 0, "Lowpass cutoff (Hz)", 20, 0, 1000)
+gen.add("lowpass_damping", double_t, 0, "Lowpass damping", sqrt(2), 0, 10)
+
 gen.add("feedback_lat",   bool_t,   0, "Enable lateral feedback?",  True)
 gen.add("feedback_ang",   bool_t,   0, "Enable angular feedback?",  False)
 

doc/second_order_lowpass_tustin.ipynb

@@ -0,0 +1,275 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# How to discretize a second order lowpass 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": [
+    "a, s, d, T, z = symbols('a,s,d,T,z')"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "First our continous time system"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# a = 2*pi*c\n",
+    "sys = 1 / (1/a**2 * s**2 + 2*d/a * s + 1)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Translate to discrete"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/latex": [
+       "$\\displaystyle \\frac{1}{1 + \\frac{4 d \\left(z - 1\\right)}{T a \\left(z + 1\\right)} + \\frac{4 \\left(z - 1\\right)^{2}}{T^{2} a^{2} \\left(z + 1\\right)^{2}}}$"
+      ],
+      "text/plain": [
+       "1/(1 + 4*d*(z - 1)/(T*a*(z + 1)) + 4*(z - 1)**2/(T**2*a**2*(z + 1)**2))"
+      ]
+     },
+     "execution_count": 4,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "sys = sys.subs(s, 2 / T * (z - 1) / (z + 1))\n",
+    "sys"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 5,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2)/(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4)\n"
+     ]
+    },
+    {
+     "data": {
+      "text/latex": [
+       "$\\displaystyle \\frac{T^{2} a^{2} z^{2} + 2 T^{2} a^{2} z + T^{2} a^{2}}{T^{2} a^{2} z^{2} + 2 T^{2} a^{2} z + T^{2} a^{2} + 4 T a d z^{2} - 4 T a d + 4 z^{2} - 8 z + 4}$"
+      ],
+      "text/plain": [
+       "(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2)/(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4)"
+      ]
+     },
+     "execution_count": 5,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "print(cancel(sys))\n",
+    "cancel(sys)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "[T**2*a**2, 2*T**2*a**2, T**2*a**2]"
+      ]
+     },
+     "execution_count": 6,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "num = Poly(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2, z)\n",
+    "num.all_coeffs()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 7,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "[T**2*a**2 + 4*T*a*d + 4, 2*T**2*a**2 - 8, T**2*a**2 - 4*T*a*d + 4]"
+      ]
+     },
+     "execution_count": 7,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "den = Poly(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4, z)\n",
+    "den.all_coeffs()"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Try to simplify"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 8,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "(b**2*z**2 + 2*b**2*z + b**2)/(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4)\n"
+     ]
+    },
+    {
+     "data": {
+      "text/latex": [
+       "$\\displaystyle \\frac{b^{2} z^{2} + 2 b^{2} z + b^{2}}{b^{2} z^{2} + 2 b^{2} z + b^{2} + 4 b d z^{2} - 4 b d + 4 z^{2} - 8 z + 4}$"
+      ],
+      "text/plain": [
+       "(b**2*z**2 + 2*b**2*z + b**2)/(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4)"
+      ]
+     },
+     "execution_count": 8,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "b = symbols('b')\n",
+    "# a*T = b -> T = b/a\n",
+    "sys = sys.subs(T, b/a)\n",
+    "print(cancel(sys))\n",
+    "cancel(sys)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 9,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "[b**2, 2*b**2, b**2]"
+      ]
+     },
+     "execution_count": 9,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "num = Poly(b**2*z**2 + 2*b**2*z + b**2, z)\n",
+    "num.all_coeffs()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 10,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "[b**2 + 4*b*d + 4, 2*b**2 - 8, b**2 - 4*b*d + 4]"
+      ]
+     },
+     "execution_count": 10,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "den = Poly(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4, z)\n",
+    "den.all_coeffs()"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Translate that to C++"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "```c++\n",
+    "auto a = 2 * M_PI * c;\n",
+    "auto b = T * a;\n",
+    "y_[0] = ((pow(b, 2)) * u_[0] + (2 * pow(b, 2)) * u_[1] + (pow(b, 2)) * u_[2] -\n",
+    "         (2 * pow(b, 2) - 8) * y_[1] - (pow(b, 2) - 4 * T * a * d + 4) * y_[2]) /\n",
+    "        (pow(b, 2) + 4 * T * a * d + 4);\n",
+    "```"
+   ]
+  }
+ ],
+ "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

@@ -9,6 +9,7 @@
 
 #include <array>
 #include <boost/noncopyable.hpp>
+#include <path_tracking_pid/details/derivative.hpp>
 #include <path_tracking_pid/details/fifo_array.hpp>
 #include <path_tracking_pid/details/second_order_lowpass.hpp>
 #include <vector>
@@ -40,9 +41,9 @@ struct ControllerState
   double tracking_error_ang = 0.0;
   // Errors with little history
   details::SecondOrderLowpass error_lat;
-  details::SecondOrderLowpass error_deriv_lat;
+  details::Derivative error_deriv_lat;
   details::SecondOrderLowpass error_ang;
-  details::SecondOrderLowpass error_deriv_ang;
+  details::Derivative error_deriv_ang;
 };
 
 class Controller : private boost::noncopyable

include/path_tracking_pid/details/derivative.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <path_tracking_pid/details/fifo_array.hpp>
+
+namespace path_tracking_pid::details
+{
+/**
+ * @brief discrete time derivative filter
+ */
+class Derivative
+{
+public:
+  /**
+   * @brief Construct a Derivative instance
+   */
+  Derivative();
+
+  /**
+   * @brief filter one sample of a signal
+   * @param u signal to be filtered
+   * @param step_size
+   * @return derivative of the signal
+   */
+  double filter(double u, double step_size);
+
+  void reset();
+
+private:
+  FifoArray<double, 2> u_ = {};
+};
+
+}  // namespace path_tracking_pid::details

include/path_tracking_pid/details/fifo_array.hpp

@@ -24,6 +24,9 @@ class FifoArray
   // Read-only access to the element at the given index.
   constexpr const value_type & operator[](std::size_t index) const { return data_[index]; }
 
+  // Read-write access to the element at the given index.
+  value_type & operator[](std::size_t index) { return data_[index]; }
+
   // Read-only access to the element at the given index (with compile-time range check).
   template <std::size_t index>
   constexpr const value_type & at() const