commit 8573856e07d17676adb4a37fa2c1d7ec58f69b3a Author: Vincent Samy Date: Tue Oct 23 10:16:10 2018 +0900 Initial commit with generic filter diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..2b7a4fe --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "cmake"] + path = cmake + url = https://github.com/jrl-umi3218/jrl-cmakemodules.git diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..cad53b0 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..e69de29 diff --git a/cmake b/cmake new file mode 160000 index 0000000..6ccc9f9 --- /dev/null +++ b/cmake @@ -0,0 +1 @@ +Subproject commit 6ccc9f9b2a2ff510ae7754c515c72f8e38410447 diff --git a/include/GenericFilter.h b/include/GenericFilter.h new file mode 100644 index 0000000..d12ecc5 --- /dev/null +++ b/include/GenericFilter.h @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include + +namespace msfc { + +namespace filt { + + class GenericFilter { + public: + GenericFilter() = default; + GenericFilter(size_t nData); + GenericFilter(size_t nData, const std::vector& aCoeff, const std::vector& bCoeff); + + void setNData(size_t nData); + void setCoeff(const std::vector& aCoeff, const std::vector& bCoeff); + void getCoeff(std::vector& aCoeff, std::vector& 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 m_aCoeff; + std::vector m_bCoeff; + + std::vector m_filteredData; + std::vector m_rawData; + // Eigen::MatrixXd m_filteredData; + // Eigen::MatrixXd m_rawData; + }; + +} // namespace filt + +} // namespace msfc \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..e330769 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/src/GenericFilter.cpp b/src/GenericFilter.cpp new file mode 100644 index 0000000..7850584 --- /dev/null +++ b/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& aCoeff, const std::vector& 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& aCoeff, const std::vector& 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& aCoeff, std::vector& 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 \ No newline at end of file