diff --git a/CMakeLists.txt b/CMakeLists.txt
index 27153211..bb137b63 100644
--- a/CMakeLists.txt
+++ b/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()
diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg
index 69c031cb..46607c36 100755
--- a/cfg/Pid.cfg
+++ b/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)
 
diff --git a/doc/integral_tustin.ipynb b/doc/integral_tustin.ipynb
new file mode 100644
index 00000000..27538c95
--- /dev/null
+++ b/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
+}
diff --git a/doc/second_order_lowpass_tustin.ipynb b/doc/second_order_lowpass_tustin.ipynb
new file mode 100644
index 00000000..9ef5eca0
--- /dev/null
+++ b/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
+}
diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp
index b36312cd..9cdb9e4d 100644
--- a/include/path_tracking_pid/controller.hpp
+++ b/include/path_tracking_pid/controller.hpp
@@ -9,7 +9,9 @@
 
 #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/integral.hpp>
 #include <path_tracking_pid/details/second_order_lowpass.hpp>
 #include <vector>
 
@@ -34,15 +36,15 @@ 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::SecondOrderLowpass error_deriv_lat;
   details::SecondOrderLowpass error_ang;
-  details::SecondOrderLowpass error_deriv_ang;
+  details::Integral error_integral_lat;
+  details::Integral error_integral_ang;
+  details::Derivative error_deriv_lat;
+  details::Derivative error_deriv_ang;
 };
 
 class Controller : private boost::noncopyable
diff --git a/include/path_tracking_pid/details/derivative.hpp b/include/path_tracking_pid/details/derivative.hpp
new file mode 100644
index 00000000..a4df7e07
--- /dev/null
+++ b/include/path_tracking_pid/details/derivative.hpp
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <path_tracking_pid/details/fifo_array.hpp>
+
+namespace path_tracking_pid::details
+{
+/**
+ * @brief Discrete time derivative filter
+ */
+class Derivative
+{
+public:
+  /**
+   * @brief Filter one sample of a signal
+   * @param u Signal to be filtered
+   * @param step_size Time step from previous sample
+   * @return Derivative of the signal
+   */
+  double filter(double u, double step_size);
+
+  /**
+   * @brief Reset the signal buffers
+   */
+  void reset();
+
+private:
+  FifoArray<double, 2> u_ = {};
+};
+
+}  // namespace path_tracking_pid::details
diff --git a/include/path_tracking_pid/details/fifo_array.hpp b/include/path_tracking_pid/details/fifo_array.hpp
index 9ded1e1f..965499e0 100644
--- a/include/path_tracking_pid/details/fifo_array.hpp
+++ b/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
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 <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
diff --git a/include/path_tracking_pid/details/second_order_lowpass.hpp b/include/path_tracking_pid/details/second_order_lowpass.hpp
index 857dadcb..bbf873f5 100644
--- a/include/path_tracking_pid/details/second_order_lowpass.hpp
+++ b/include/path_tracking_pid/details/second_order_lowpass.hpp
@@ -1,29 +1,55 @@
 #pragma once
 
+#include <limits>
 #include <path_tracking_pid/details/fifo_array.hpp>
 
 namespace path_tracking_pid::details
 {
-// Error tracker for the last 3 error and filtered error values.
+/**
+ * @brief Discrete time second order lowpass filter
+ */
 class SecondOrderLowpass
 {
+  constexpr static auto NaN = std::numeric_limits<double>::quiet_NaN();
+
 public:
-  // Pushes the given value to the errors FIFO buffer. A corresponding filtered error value is calculated and pushed
-  // to the filtered errors FIFO buffer.
-  void push(double value);
+  /**
+   * @brief Construct a SecondOrderLowpass instance with NaNs
+   */
+  SecondOrderLowpass() = default;
 
-  // Resets both errors and filtered errors FIFO buffers.
-  void reset();
+  /**
+   * @brief Construct a SecondOrderLowpass instance
+   * @param cutoff frequency in Hz, 0 disables the filter
+   * @param damping frequency in Hz
+   */
+  SecondOrderLowpass(double cutoff, double damping);
 
-  // Read-only access to the errors FIFO buffer.
-  const FifoArray<double, 3> & errors() const;
+  /**
+   * @brief Change the parameters of the filter
+   * @param cutoff frequency in Hz, 0 disables the filter
+   * @param damping frequency in Hz
+   */
+  void configure(double cutoff, double damping);
 
-  // Read-only access to the filtered errors FIFO buffer.
-  const FifoArray<double, 3> & filtered_errors() const;
+  /**
+   * @brief Filter one sample of a signal
+   * @param u Signal to be filtered
+   * @param step_size Time step from previous sample
+   * @return Lowpass-filtered signal
+   */
+  double filter(double u, double step_size);
+
+  /**
+   * @brief Reset the signal buffers
+   */
+  void reset();
 
 private:
-  FifoArray<double, 3> errors_;
-  FifoArray<double, 3> filtered_errors_;
+  FifoArray<double, 3> u_ = {};
+  FifoArray<double, 3> y_ = {};
+  double cutoff_ = NaN;
+  double damping_ = NaN;
 };
 
 }  // namespace path_tracking_pid::details
diff --git a/param/controllers.yaml b/param/controllers.yaml
index 5d563b7c..3fe1be86 100644
--- a/param/controllers.yaml
+++ b/param/controllers.yaml
@@ -15,4 +15,5 @@ PathTrackingPID:
   abs_minimum_x_vel: 0.0
   anti_collision: true
   use_mpc: false
+  feedforward_ang: true
   controller_debug_enabled: true
diff --git a/src/controller.cpp b/src/controller.cpp
index f94396a5..7912d888 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -399,8 +399,9 @@ Controller::UpdateResult Controller::update(
     ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability.");
   }
 
-  controller_state_.error_lat.push(error.getOrigin().y());
-  controller_state_.error_ang.push(angles::normalize_angle(tf2::getYaw(error.getRotation())));
+  auto error_lat_filtered = controller_state_.error_lat.filter(error.getOrigin().y(), dt.toSec());
+  auto error_ang_filtered = controller_state_.error_ang.filter(
+    angles::normalize_angle(tf2::getYaw(error.getRotation())), dt.toSec());
 
   // tracking error for diagnostic purposes
   // Transform current pose into local-path-frame to get tracked-frame-error
@@ -422,38 +423,27 @@ Controller::UpdateResult Controller::update(
 
   // trackin_error here represents the error between tracked link and position on plan
   controller_state_.tracking_error_lat = current_tracking_err.y();
-  controller_state_.tracking_error_ang = controller_state_.error_ang.errors().at<0>();
+  controller_state_.tracking_error_ang = angles::normalize_angle(tf2::getYaw(error.getRotation())),
+  dt.toSec();
 
   // integrate the error
-  controller_state_.error_integral_lat += controller_state_.error_lat.errors().at<0>() * dt.toSec();
-  controller_state_.error_integral_ang += controller_state_.error_ang.errors().at<0>() * 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:
-  controller_state_.error_deriv_lat.push(
-    (controller_state_.error_lat.errors().at<0>() - controller_state_.error_lat.errors().at<1>()) /
-    dt.toSec());
-  controller_state_.error_deriv_ang.push(
-    (controller_state_.error_ang.errors().at<0>() - controller_state_.error_ang.errors().at<1>()) /
-    dt.toSec());
+  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 * controller_state_.error_lat.filtered_errors().at<0>();
-  const auto integral_lat = config_.Ki_lat * controller_state_.error_integral_lat;
-  const auto derivative_lat =
-    config_.Kd_lat * controller_state_.error_deriv_lat.filtered_errors().at<0>();
-
-  const auto proportional_ang =
-    config_.Kp_ang * controller_state_.error_ang.filtered_errors().at<0>();
-  const auto integral_ang = config_.Ki_ang * controller_state_.error_integral_ang;
-  const auto derivative_ang =
-    config_.Kd_ang * controller_state_.error_deriv_ang.filtered_errors().at<0>();
+  const auto proportional_lat = config_.Kp_lat * error_lat_filtered;
+  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 * error_integral_ang;
+  const auto derivative_ang = config_.Kd_ang * error_deriv_ang;
 
   /***** Compute forward velocity *****/
   // Apply acceleration limits and end velocity
@@ -615,8 +605,8 @@ Controller::UpdateResult Controller::update(
   // Populate debug output
   // Error topic containing the 'control' error on which the PID acts
   result.pid_debug.control_error.linear.x = 0.0;
-  result.pid_debug.control_error.linear.y = controller_state_.error_lat.errors().at<0>();
-  result.pid_debug.control_error.angular.z = controller_state_.error_ang.errors().at<0>();
+  result.pid_debug.control_error.linear.y = error_lat_filtered;
+  result.pid_debug.control_error.angular.z = error_ang_filtered;
   // Error topic containing the 'tracking' error, i.e. the real error between path and tracked link
   result.pid_debug.tracking_error.linear.x = 0.0;
   result.pid_debug.tracking_error.linear.y = controller_state_.tracking_error_lat;
@@ -734,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;
@@ -820,7 +810,7 @@ double Controller::mpc_based_max_vel(
     // max_target_x_vel we can increase it
     if (
       mpc_fwd_iter == config_.mpc_max_fwd_iterations &&
-      fabs(controller_state_.error_lat.errors().at<0>()) <= config_.mpc_max_error_lat &&
+      fabs(controller_state_.tracking_error_lat) <= config_.mpc_max_error_lat &&
       fabs(new_nominal_x_vel) < abs(target_x_vel)) {
       mpc_vel_optimization_iter += 1;
 
@@ -843,7 +833,7 @@ double Controller::mpc_based_max_vel(
       mpc_fwd_iter = 0;
     }
     // If the robot gets out of bounds earlier we decrease the velocity
-    else if (abs(controller_state_.error_lat.errors().at<0>()) >= config_.mpc_max_error_lat) {
+    else if (abs(controller_state_.tracking_error_lat) >= config_.mpc_max_error_lat) {
       mpc_vel_optimization_iter += 1;
 
       // Lower speed
@@ -927,6 +917,11 @@ void Controller::printParameters() const
 
 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();
   controller_state_.error_deriv_lat.reset();
@@ -978,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();
 }
 
diff --git a/src/details/derivative.cpp b/src/details/derivative.cpp
new file mode 100644
index 00000000..25dc4f63
--- /dev/null
+++ b/src/details/derivative.cpp
@@ -0,0 +1,13 @@
+#include <path_tracking_pid/details/derivative.hpp>
+
+namespace path_tracking_pid::details
+{
+double Derivative::filter(double u, double step_size)
+{
+  // save history
+  u_.push(u);
+  return (u_[0] - u_[1]) / step_size;
+}
+
+void Derivative::reset() { u_ = {}; }
+}  // namespace path_tracking_pid::details
diff --git a/src/details/integral.cpp b/src/details/integral.cpp
new file mode 100644
index 00000000..3b8997cb
--- /dev/null
+++ b/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
diff --git a/src/details/second_order_lowpass.cpp b/src/details/second_order_lowpass.cpp
index 8614c5f0..3da54727 100644
--- a/src/details/second_order_lowpass.cpp
+++ b/src/details/second_order_lowpass.cpp
@@ -3,34 +3,43 @@
 
 namespace path_tracking_pid::details
 {
-namespace
+SecondOrderLowpass::SecondOrderLowpass(double cutoff, double damping)
+: cutoff_(cutoff), damping_(damping)
 {
-// Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at 1/4 of the sample rate.
-constexpr double cutoff = 1.;
-
-}  // namespace
+}
 
-void SecondOrderLowpass::push(double value)
+void SecondOrderLowpass::configure(double cutoff, double damping)
 {
-  errors_.push(value);
-
-  filtered_errors_.push(
-    (1 / (1 + cutoff * cutoff + M_SQRT2 * cutoff)) *
-    (errors_.at<2>() + 2 * errors_.at<1>() + errors_.at<0>() -
-     (cutoff * cutoff - M_SQRT2 * cutoff + 1) * filtered_errors_.at<1>() -
-     (-2 * cutoff * cutoff + 2) * filtered_errors_.at<0>()));
+  cutoff_ = cutoff;
+  damping_ = damping;
 }
 
-void SecondOrderLowpass::reset()
+double SecondOrderLowpass::filter(double u, double step_size)
 {
-  errors_.reset();
-  filtered_errors_.reset();
+  // save history
+  u_.push(u);
+  y_.push(u);  // increase index so the math below looks correct
+
+  if (cutoff_ == 0) {
+    return u;
+  }
+
+  // A continous time second order lowpass was discretized with Tustin's method. For a mathematical
+  // explanation, see doc/second_order_lowpass_tustin.ipynb
+  auto c = cutoff_;
+  auto d = damping_;
+  auto T = step_size;
+  auto a = 2 * M_PI * c;
+  auto b = T * a;
+  y_[0] = ((pow(b, 2)) * u_[0] + (2 * pow(b, 2)) * u_[1] + (pow(b, 2)) * u_[2] -
+           (2 * pow(b, 2) - 8) * y_[1] - (pow(b, 2) - 4 * T * a * d + 4) * y_[2]) /
+          (pow(b, 2) + 4 * T * a * d + 4);
+  return y_[0];
 }
 
-const FifoArray<double, 3> & SecondOrderLowpass::errors() const { return errors_; }
-const FifoArray<double, 3> & SecondOrderLowpass::filtered_errors() const
+void SecondOrderLowpass::reset()
 {
-  return filtered_errors_;
+  u_ = {};
+  y_ = {};
 }
-
 }  // namespace path_tracking_pid::details
diff --git a/test/test_path_tracking_pid.py b/test/test_path_tracking_pid.py
index aa4efb8e..74ec6f68 100755
--- a/test/test_path_tracking_pid.py
+++ b/test/test_path_tracking_pid.py
@@ -104,7 +104,7 @@ def test_exepath_action(self):
             rospy.sleep(10.0)
             self.assertTrue(checker.slowed_down)
 
-        finished_in_time = client.wait_for_result(timeout=rospy.Duration(60))
+        finished_in_time = client.wait_for_result(timeout=rospy.Duration(120))
         self.assertTrue(finished_in_time, msg="Action call didn't return in time")
         self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome")
 
diff --git a/test/unittests/test_derivative.cpp b/test/unittests/test_derivative.cpp
new file mode 100644
index 00000000..23f033e5
--- /dev/null
+++ b/test/unittests/test_derivative.cpp
@@ -0,0 +1,34 @@
+#include <gtest/gtest.h>
+
+#include <path_tracking_pid/details/derivative.hpp>
+#include <vector>
+
+using path_tracking_pid::details::Derivative;
+
+constexpr double eps = 1e-6;
+
+TEST(Derivative, StepResponse)
+{
+  double dt = 0.1;
+
+  Derivative filter;
+
+  std::vector<double> expected_response = {10, 0, 0};
+  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);
+  }
+}
+
+TEST(Derivative, Reset)
+{
+  double dt = 0.1;
+
+  Derivative filter;
+
+  EXPECT_NEAR(filter.filter(1, dt), 10, eps);
+  filter.reset();
+  EXPECT_NEAR(filter.filter(0, dt), 0, eps);
+}
diff --git a/test/unittests/test_fifo_array.cpp b/test/unittests/test_fifo_array.cpp
index 42a1562b..bbf8cfdb 100644
--- a/test/unittests/test_fifo_array.cpp
+++ b/test/unittests/test_fifo_array.cpp
@@ -4,7 +4,6 @@
 
 namespace
 {
-
 using path_tracking_pid::details::FifoArray;
 
 TEST(PathTrackingPidDetailsFifoArray, Initialize)
@@ -114,4 +113,14 @@ TEST(PathTrackingPidDetailsFifoArray, OtherSize)
   EXPECT_EQ(fifo[4], 2);
 }
 
+TEST(PathTrackingPidDetailsFifoArray, Assign)
+{
+  FifoArray<int, 3> fifo;
+  fifo[0] = 1;
+
+  EXPECT_EQ(fifo[0], 1);
+  EXPECT_EQ(fifo[1], 0);
+  EXPECT_EQ(fifo[2], 0);
+}
+
 }  // namespace
diff --git a/test/unittests/test_integral.cpp b/test/unittests/test_integral.cpp
new file mode 100644
index 00000000..1032cfc9
--- /dev/null
+++ b/test/unittests/test_integral.cpp
@@ -0,0 +1,53 @@
+#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);
+  }
+}
+
+TEST(Integral, Reset)
+{
+  double dt = 0.1;
+  double windup_limit = 0.5;
+
+  Integral filter{windup_limit};
+
+  EXPECT_NEAR(filter.filter(1, dt), 0.05, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.15, eps);
+  filter.reset();
+  EXPECT_NEAR(filter.filter(1, dt), 0.05, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.15, eps);
+}
+
+TEST(Integral, Configure)
+{
+  double dt = 0.1;
+  double windup_limit = 0.2;
+
+  Integral filter{windup_limit};
+
+  EXPECT_NEAR(filter.filter(1, dt), 0.05, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.15, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.20, eps);
+  filter.configure(0.35);
+  EXPECT_NEAR(filter.filter(1, dt), 0.30, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.35, eps);
+}
diff --git a/test/unittests/test_second_order_lowpass.cpp b/test/unittests/test_second_order_lowpass.cpp
new file mode 100644
index 00000000..93d23ea8
--- /dev/null
+++ b/test/unittests/test_second_order_lowpass.cpp
@@ -0,0 +1,133 @@
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <path_tracking_pid/details/second_order_lowpass.hpp>
+#include <vector>
+
+using path_tracking_pid::details::SecondOrderLowpass;
+
+constexpr double eps = 1e-6;
+
+TEST(SecondOrderLowpass, StepResponse)
+{
+  double dt = 0.1;
+  double cutoff = 1 / dt / 4;
+  double damping = sqrt(2);
+
+  SecondOrderLowpass filter(cutoff, damping);
+
+  std::vector<double> expected_response = {0.16071,  0.514214, 0.770813, 0.877725, 0.939488,
+                                           0.968659, 0.984211, 0.991911, 0.995898, 0.997907};
+  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);
+  }
+}
+
+TEST(SecondOrderLowpass, Disable)
+{
+  double dt = 0.1;
+  double cutoff = 0;
+  double damping = sqrt(2);
+
+  SecondOrderLowpass filter(cutoff, damping);
+  EXPECT_NEAR(filter.filter(0, dt), 0, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 1, eps);
+  EXPECT_NEAR(filter.filter(5, dt), 5, eps);
+}
+
+TEST(SecondOrderLowpass, StepResponseCutoff)
+{
+  double dt = 0.1;
+  double cutoff = 1 / dt / 8;  // lower cutoff so slower response
+  double damping = sqrt(2);
+
+  SecondOrderLowpass filter(cutoff, damping);
+
+  std::vector<double> expected_response = {
+    0.068087, 0.255112, 0.461572, 0.612177, 0.720691,
+    0.798844, 0.855129, 0.895665, 0.924859, 0.945884,
+  };
+  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);
+  }
+}
+
+TEST(SecondOrderLowpass, StepResponseDamping)
+{
+  double dt = 0.1;
+  double cutoff = 1 / dt / 4;
+  double damping = sqrt(2) * 2;  // more damping
+
+  SecondOrderLowpass filter(cutoff, damping);
+
+  std::vector<double> expected_response = {
+    0.101795, 0.318258, 0.494899, 0.618187, 0.716157,
+    0.786043, 0.84057,  0.880057, 0.91048,  0.932743,
+  };
+  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);
+  }
+}
+
+TEST(SecondOrderLowpass, Reset)
+{
+  double dt = 0.1;
+  double cutoff = 1 / dt / 4;
+  double damping = sqrt(2);
+
+  SecondOrderLowpass filter(cutoff, damping);
+
+  std::vector<double> expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488};
+  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);
+  }
+
+  filter.reset();
+
+  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);
+  }
+}
+
+TEST(SecondOrderLowpass, Configure)
+{
+  double dt = 0.1;
+  double cutoff = 1 / dt / 4;
+  double damping = sqrt(2);
+
+  SecondOrderLowpass filter(cutoff, damping);
+
+  std::vector<double> expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488};
+  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);
+  }
+
+  filter.configure(cutoff / 2, damping);
+
+  // no configure step response is {0.968659, 0.984211, 0.991911, 0.995898, 0.997907}
+  expected_response = {0.957154, 0.969162, 0.977792, 0.984006, 0.988481};
+  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);
+  }
+}