Initial commit with generic filter

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "cmake"]
+	path = cmake
+	url = https://github.com/jrl-umi3218/jrl-cmakemodules.git

CMakeLists.txt

@@ -0,0 +1,30 @@
+# Version minimum
+cmake_minimum_required(VERSION 3)
+
+include(cmake/base.cmake)
+include(cmake/eigen.cmake)
+
+set(PROJECT_NAME fratio)
+set(PROJECT_DESCRIPTION "Filter using rational transfer function")
+set(PROJECT_URL "...")
+
+#SET(CXX_DISABLE_WERROR True)
+set(DOXYGEN_USE_MATHJAX "NO")
+
+project(${PROJECT_NAME} CXX)
+set(CMAKE_CXX_STANDARD 14)
+
+setup_project()
+
+# for MSVC
+if(MSVC)
+    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /W1 /MP")
+endif()
+
+# Eigen
+set(Eigen_REQUIRED "eigen3 >= 3.3")
+search_for_eigen()
+
+add_subdirectory(src)
+
+setup_project_finalize()

cmake

@@ -0,0 +1 @@
+Subproject commit 6ccc9f9b2a2ff510ae7754c515c72f8e38410447

include/GenericFilter.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <stddef.h>
+#include <vector>
+
+namespace msfc {
+
+namespace filt {
+
+    class GenericFilter {
+    public:
+        GenericFilter() = default;
+        GenericFilter(size_t nData);
+        GenericFilter(size_t nData, const std::vector<double>& aCoeff, const std::vector<double>& bCoeff);
+
+        void setNData(size_t nData);
+        void setCoeff(const std::vector<double>& aCoeff, const std::vector<double>& bCoeff);
+        void getCoeff(std::vector<double>& aCoeff, std::vector<double>& bCoeff) const noexcept;
+        size_t filterOrder() const noexcept { return m_order; }
+
+        // bool stepFilter(const Eigen::VectorXd& data);
+        bool stepFilter(double data);
+
+        // Eigen::VectorXd results() const noexcept;
+        double results() const noexcept;
+
+    private:
+        void normalize();
+        void shiftData();
+
+    protected:
+        size_t m_nACoeffFilteredData;
+        size_t m_nBCoeffFilteredData;
+        std::vector<double> m_aCoeff;
+        std::vector<double> m_bCoeff;
+
+        std::vector<double> m_filteredData;
+        std::vector<double> m_rawData;
+        // Eigen::MatrixXd m_filteredData;
+        // Eigen::MatrixXd m_rawData;
+    };
+
+} // namespace filt
+
+} // namespace msfc

src/CMakeLists.txt

@@ -0,0 +1,13 @@
+set(HEADERS
+    ../include/GenericFilter.h
+)
+
+set(SRC
+    GenericFilter.cpp
+)
+
+add_library(${PROJECT_NAME} ${HEADERS} ${SRC})
+install(TARGETS ${PROJECT_NAME}
+    RUNTIME DESTINATION bin
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib)

src/GenericFilter.cpp

@@ -0,0 +1,126 @@
+#include "msfc/filter/GenericFilter.h"
+#include "msfc/logging.h"
+
+namespace msfc {
+
+namespace filt {
+
+    GenericFilter::GenericFilter(size_t nData)
+    // : m_filteredData(nData, order)
+    // , m_rawData(nData, order)
+    {
+    }
+
+    GenericFilter::GenericFilter(size_t nData, const std::vector<double>& aCoeff, const std::vector<double>& bCoeff)
+        : m_aCoeff(aCoeff)
+        , m_bCoeff(bCoeff)
+        , m_filteredData(aCoeff.size())
+        , m_rawData(bCoeff.size())
+    {
+        assert(aCoeff.size() > 0);
+        assert(bCoeff.size() > 0);
+        normalize();
+    }
+
+    void GenericFilter::setNData(size_t nData)
+    {
+        // m_filteredData.resize(nData, m_filteredData.cols());
+        // m_rawData.resize(nData, nData.cols());
+    }
+
+    void GenericFilter::setCoeff(const std::vector<double>& aCoeff, const std::vector<double>& bCoeff)
+    {
+        assert(aCoeff.size() > 0);
+        assert(bCoeff.size() > 0);
+
+        m_nACoeffFilteredData = 0;
+        m_nBCoeffFilteredData = 0;
+        m_aCoeff = aCoeff;
+        m_bCoeff = bCoeff;
+        m_filteredData.resize(aCoeff.size());
+        m_rawData.resize(bCoeff.size());
+
+        normalize();
+    }
+
+    void GenericFilter::getCoeff(std::vector<double>& aCoeff, std::vector<double>& bCoeff) const noexcept
+    {
+        aCoeff = m_aCoeff;
+        bCoeff = m_bCoeff;
+    }
+
+    // bool GenericFilter::stepFilter(const Eigen::VectorXd& data)
+    // {
+    //     if (m_filteredData.rows() != data.size()) {
+    //         LOG_ERROR("Bad data size. Expected vector of size " << m_filteredData.rows() << ", got a vector of size " << data.size());
+    //         return false;
+    //     }
+
+    //     if (m_nFilteredData == 0) {
+    //         m_rawData.row(0) = data;
+    //         m_filteredData.row(0).noalias() = m_bCoeff(0) * data;
+    //         ++m_nFilteredData;
+    //     } else if (m_nFilteredData < m_order) {
+    //         m_filteredData.row(m_nFilteredData).noalias() = m_bCoeff(0) * data;
+    //         for (size_t i = 1; i <= m_nFilteredData; ++i) {
+    //             m_filteredData.row(m_nFilteredData).noalias() += m_bCoeff(i) * m_rawData.row(i - 1);
+    //         }
+    //         ++m_nFilteredData;
+    //     }
+
+    //     if (m_nFilteredData >= m_order) {
+
+    //     } else {
+    //         for (size_t i = 0; i < m_nFilteredData; ++i) {
+    //         }
+
+    //         ++m_nFilteredData;
+    //     }
+    // }
+
+    // https://stackoverflow.com/questions/50511549/meaning-of-rational-transfer-function-underlying-matlab-filter-or-scipy-signal-f
+    bool GenericFilter::stepFilter(double data)
+    {
+        double filtData;
+        for (size_t i = 0; i < m_nBCoeffFilteredData; ++i)
+            filtData += m_bCoeff[i] * m_rawData[m_nBCoeffFilteredData - i];
+        for (size_t i = 1; i < m_nACoeffFilteredData; ++i)
+            filtData -= m_aCoeff[i] * m_filteredData[m_nACoeffFilteredData - i];
+
+        m_filteredData[m_nACoeffFilteredData] = filtData;
+        m_rawData[m_nBCoeffFilteredData] = data;
+        ++m_nACoeffFilteredData;
+        ++m_nBCoeffFilteredData;
+
+        if (m_nACoeffFilteredData == m_filteredData.size()) {
+            double* fd = m_filteredData.data();
+            for (size_t i = 0; i < m_filteredData.size() - 1; ++i)
+                *(fd) = *(++fd);
+
+            --m_nACoeffFilteredData;
+        }
+
+        if (m_nBCoeffFilteredData == m_rawData.size()) {
+            double* rd = m_rawData.data();
+            for (size_t i = 0; i < m_rawData.size() - 1; ++i)
+                *(rd) = *(++rd);
+
+            --m_nBCoeffFilteredData;
+        }
+    }
+
+    void GenericFilter::normalize()
+    {
+        double a0 = m_aCoeff.front();
+        if (std::abs(a0) < 1e-6) // Divide by zero
+            LOG_ERROR_AND_THROW(std::invalid_argument, "By filtering value for coefficient a0. Should be superior to 1e-6");
+
+        for (double& a : m_aCoeff)
+            a /= a0;
+        for (double& b : m_bCoeff)
+            b /= a0;
+    }
+
+} // namespace filt
+
+} // namespace msfc