diff --git a/CMakeLists.txt b/CMakeLists.txt
index a6bfc3c0..193ff96a 100644
--- a/CMakeLists.txt
+++ b/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/second_order_lowpass.cpp
     test/unittests/test_main.cpp
     test/unittests/test_fifo_array.cpp
-    test/unittests/test_calculations.cpp)
+    test/unittests/test_calculations.cpp
+  )
   target_link_libraries(unittests ${catkin_LIBRARIES} ${PROJECT_NAME})
 endif()
diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg
index cc0135e6..aad4e3e2 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)", 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)
 
diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp
index ea41e582..16cda5a8 100644
--- a/include/path_tracking_pid/controller.hpp
+++ b/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
diff --git a/include/path_tracking_pid/details/derivative.hpp b/include/path_tracking_pid/details/derivative.hpp
new file mode 100644
index 00000000..0cc528a2
--- /dev/null
+++ b/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
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/second_order_lowpass.hpp b/include/path_tracking_pid/details/second_order_lowpass.hpp
index 857dadcb..e875428a 100644
--- a/include/path_tracking_pid/details/second_order_lowpass.hpp
+++ b/include/path_tracking_pid/details/second_order_lowpass.hpp
@@ -4,26 +4,41 @@
 
 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
 {
 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();
 
-  // Resets both errors and filtered errors FIFO buffers.
-  void reset();
+  /**
+   * @brief Construct a SecondOrderLowpass instance
+   * @param fden frequency in Hz
+   * @param bden frequency in Hz
+   */
+  SecondOrderLowpass(double fden, double bden);
+
+  void configure(double fden, double bden);
 
-  // Read-only access to the errors FIFO buffer.
-  const FifoArray<double, 3> & errors() const;
+  /**
+   * @brief filter one sample of a signal
+   * @param u signal to be filtered
+   * @param step_size
+   * @return lowpass-filtered signal
+   */
+  double filter(double u, double step_size);
 
-  // Read-only access to the filtered errors FIFO buffer.
-  const FifoArray<double, 3> & filtered_errors() const;
+  void reset();
 
 private:
-  FifoArray<double, 3> errors_;
-  FifoArray<double, 3> filtered_errors_;
+  FifoArray<double, 3> u_ = {};
+  FifoArray<double, 3> y_ = {};
+  double fden_;
+  double bden_;
 };
 
 }  // namespace path_tracking_pid::details
diff --git a/second_order_lowpass_tustin.ipynb b/second_order_lowpass_tustin.ipynb
new file mode 100644
index 00000000..9ef5eca0
--- /dev/null
+++ b/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/src/controller.cpp b/src/controller.cpp
index 588571bd..fe6bc822 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -445,8 +445,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
@@ -468,11 +469,12 @@ 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();
+  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 =
@@ -481,25 +483,17 @@ Controller::UpdateResult Controller::update(
     std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit);
 
   // 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 proportional_lat = config_.Kp_lat * error_lat_filtered;
   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 derivative_lat = config_.Kd_lat * error_deriv_lat;
 
-  const auto proportional_ang =
-    config_.Kp_ang * controller_state_.error_ang.filtered_errors().at<0>();
+  const auto proportional_ang = config_.Kp_ang * error_ang_filtered;
   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 derivative_ang = config_.Kd_ang * error_deriv_ang;
 
   /***** Compute forward velocity *****/
   // Apply acceleration limits and end velocity
@@ -662,8 +656,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;
@@ -867,7 +861,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;
 
@@ -890,7 +884,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
@@ -974,6 +968,9 @@ 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);
+
   // Erase all queues when config changes
   controller_state_.error_lat.reset();
   controller_state_.error_deriv_lat.reset();
diff --git a/src/details/derivative.cpp b/src/details/derivative.cpp
new file mode 100644
index 00000000..4bcf23cb
--- /dev/null
+++ b/src/details/derivative.cpp
@@ -0,0 +1,15 @@
+#include <path_tracking_pid/details/derivative.hpp>
+
+namespace path_tracking_pid::details
+{
+Derivative::Derivative() {}
+
+double Derivative::filter(double u, double step_size)
+{
+  // save history
+  u_.push(u);
+  return (u_[0] - u_[1]) / step_size;
+}
+
+void Derivative::reset() {}
+}  // namespace path_tracking_pid::details
diff --git a/src/details/second_order_lowpass.cpp b/src/details/second_order_lowpass.cpp
index 8614c5f0..6a2c4e1a 100644
--- a/src/details/second_order_lowpass.cpp
+++ b/src/details/second_order_lowpass.cpp
@@ -1,36 +1,44 @@
+/**
+(c) Rene' van de Molengraft, 2000
+*/
+
 #include <cmath>
 #include <path_tracking_pid/details/second_order_lowpass.hpp>
 
 namespace path_tracking_pid::details
 {
-namespace
-{
-// Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at 1/4 of the sample rate.
-constexpr double cutoff = 1.;
+constexpr auto NaN = std::numeric_limits<float>::quiet_NaN();
 
-}  // namespace
+SecondOrderLowpass::SecondOrderLowpass() : fden_(NaN), bden_(NaN) {}
 
-void SecondOrderLowpass::push(double value)
-{
-  errors_.push(value);
+SecondOrderLowpass::SecondOrderLowpass(double fden, double bden) : fden_(fden), bden_(bden) {}
 
-  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>()));
+void SecondOrderLowpass::configure(double fden, double bden)
+{
+  fden_ = fden;
+  bden_ = bden;
 }
 
-void SecondOrderLowpass::reset()
+double SecondOrderLowpass::filter(double u, double step_size)
 {
-  errors_.reset();
-  filtered_errors_.reset();
+  // save history
+  u_.push(u);
+  y_.push(0);  // increase index so the math below looks correct
+
+  auto c = fden_;
+  auto d = bden_;
+  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/unittests/second_order_lowpass.cpp b/test/unittests/second_order_lowpass.cpp
new file mode 100644
index 00000000..d4def600
--- /dev/null
+++ b/test/unittests/second_order_lowpass.cpp
@@ -0,0 +1,24 @@
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <path_tracking_pid/details/second_order_lowpass.hpp>
+
+using path_tracking_pid::details::SecondOrderLowpass;
+
+TEST(SecondOrderLowpass, StepResponse)
+{
+  double dt = 0.1;
+  double crossover = 1 / dt / 4;
+  double damping = sqrt(2);
+
+  SecondOrderLowpass filter(crossover, 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], 1e-6);
+  }
+}