[RFC] Filter Development

include/kodlab_mjbots_sdk/filter.h

@@ -0,0 +1,43 @@
+/**
+ * @file filter.h
+ * @author Lucien C. Peach ([email protected])
+ * @brief Abstract base class for support of filter implementation. 
+ * @date 9/10/2022
+ * 
+ * @copyright 2022 The Trustees of the University of Pennsylvania. All rights reserved.
+ * 
+ */
+
+#pragma once
+
+#include "Eigen/Geometry"
+
+namespace kodlab
+{
+
+class Filter {
+
+    public: 
+
+        // Pure virtual function for filter update calls
+        virtual Eigen::Vector3f Update(Eigen::Vector3f raw_data) = 0;
+
+        // Accessor for raw_data_
+        Eigen::Vector3f GetRawData() {
+            return raw_data_;
+        }
+
+        // Accessor for filtered_data_
+        Eigen::Vector3f GetFilteredData() {
+            return filtered_data_;
+        }
+
+   protected:
+
+        Eigen::Vector3f raw_data_;
+        Eigen::Vector3f filtered_data_;
+
+};
+
+} // kodlab::mjbots
+

include/kodlab_mjbots_sdk/low_pass_filter.h

@@ -0,0 +1,67 @@
+/**
+ * @file low_pass_filter.h
+ * @author Lucien C. Peach ([email protected])
+ * @brief A discrete-time low-pass filter class.
+ * @date 9/10/2022
+ * 
+ * @copyright 2022 The Trustees of the University of Pennsylvania. All rights reserved.
+ * 
+ */
+
+#pragma once
+
+#include "kodlab_mjbots_sdk/filter.h"
+#include "Eigen/Geometry"
+
+namespace kodlab
+{
+
+class LowPassFilter : public Filter {
+
+    public:
+
+        LowPassFilter(float dt, float k_frequency_cutoff, Eigen::Vector3f raw_data);
+
+        float FilterGain(const float k_frequency_cutoff);
+
+        Eigen::Vector3f Update(Eigen::Vector3f raw_data) override;
+
+        // Accessor for dt_
+        float GetDt() {
+            return dt_;
+        }
+
+        // Accessor for k_frequency_cutoff_
+        float GetCutoffFreq() {
+            return k_frequency_cutoff_;
+        }
+
+        // Accessor for alpha_
+        float GetAlpha() {
+            return alpha_;
+        }
+
+
+    private:
+
+        float dt_;
+        float k_frequency_cutoff_;
+        float alpha_;
+
+};
+
+} //kodlab
+
+
+
+/* Notes for further additions
+
+//main.cpp
+
+LowPassFilter<Eigen::Vector3f> lpf  (dt, cutoff );
+while (True){
+    //DO STUFF
+    filt_data = lpf.Update(data);
+
+    //DO STUFF
+} */

src/low_pass_filter.cpp

@@ -0,0 +1,45 @@
+/**
+ * @file low_pass_filter.cpp
+ * @author Lucien C. Peach ([email protected])
+ * @brief Implementation of a discrete-time low-pass filter class.
+ * @date 9/10/2022
+ * 
+ * @copyright 2022 The Trustees of the University of Pennsylvania. All rights reserved.
+ * 
+ */
+
+#include "kodlab_mjbots_sdk/low_pass_filter.h"
+#include <math.h>
+#include <Eigen/Geometry>
+
+namespace kodlab
+{
+
+LowPassFilter::LowPassFilter(float dt, float k_frequency_cutoff, Eigen::Vector3f raw_data) {
+
+    dt_ = dt;
+    raw_data_ = raw_data;
+    filtered_data_ = raw_data;
+    k_frequency_cutoff_ = k_frequency_cutoff; 
+
+};
+
+float LowPassFilter::FilterGain(float k_frequency_cutoff_) {
+
+    float k_filter_time_constant = 1.0 / (2 * M_PI * k_frequency_cutoff_);
+
+    float alpha_ = dt_ / (k_filter_time_constant + dt_);
+
+}
+
+Eigen::Vector3f LowPassFilter::Update(Eigen::Vector3f raw_data) {
+
+    filtered_data_ = alpha_ * raw_data + (1 - alpha_) * filtered_data_;
+    raw_data = raw_data;
+
+}
+
+// Future Version: Include Reinitialize / Set Filter Data: 
+
+} // kodlab
+