utilizing filters.hpp

Signed-off-by: silanus23 <berkantali23@outlook.com>

Author: silanus23

control_toolbox/include/control_filters/exponential_filter.hpp

@@ -24,7 +24,6 @@
 
 #include "control_toolbox/exponential_filter.hpp"
 #include "control_toolbox/exponential_filter_parameters.hpp"
-#include "control_toolbox/filters.hpp"
 
 namespace control_filters
 {

control_toolbox/include/control_toolbox/exponential_filter.hpp

@@ -24,6 +24,7 @@
 #include <vector>
 
 #include "control_toolbox/filter_traits.hpp"
+#include "control_toolbox/filters.hpp"
 
 namespace control_toolbox
 {
@@ -101,8 +102,8 @@ bool ExponentialFilter<T>::update(const T & data_in, T & data_out)
   StorageType storage_in;
   Traits::assign(storage_in, data_in);
 
-  // Exponential filter update: y[n] = α * x[n] + (1-α) * y[n-1]
-  old_value_ = storage_in * alpha_ + old_value_ * (1.0 - alpha_);
+  // Exponential filter update using the templated exponentialSmoothing function
+  old_value_ = filters::exponentialSmoothing(storage_in, old_value_, alpha_);
 
   Traits::assign(data_out, old_value_);
   Traits::add_metadata(data_out, data_in);

control_toolbox/include/control_toolbox/filters.hpp

@@ -40,11 +40,11 @@ namespace filters
 /** Exponential smoothing filter. Alpha is between 0 and 1.
  * Values closer to 0 weight the last smoothed value more heavily */
 
-static inline double exponentialSmoothing(
-  double current_raw_value, double last_smoothed_value, double alpha)
+template <typename T>
+static inline T exponentialSmoothing(const T & current_raw_value, const T & last_smoothed_value, double alpha)
 {
   return alpha * current_raw_value + (1 - alpha) * last_smoothed_value;
 }
 }  // namespace filters
 
-#endif  // CONTROL_TOOLBOX__FILTERS_HPP_"
+#endif  // CONTROL_TOOLBOX__FILTERS_HPP_