Fix lowpass & refactor filter structure

CMakeLists.txt

@@ -64,8 +64,10 @@ 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/integral.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})
@@ -108,8 +110,12 @@ 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_derivative.cpp
     test/unittests/test_fifo_array.cpp
-    test/unittests/test_calculations.cpp)
+    test/unittests/test_integral.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), 0 disables the filter", 0, 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/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
+}