Fix lowpass & refactor filter structure

- Fixed second order lowpass
- 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/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()

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)
 

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

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