From 47d7e870231a9c5b7acf16decbbb2333b3b45009 Mon Sep 17 00:00:00 2001 From: Erki Date: Sun, 11 Dec 2022 23:58:39 +0200 Subject: [PATCH] Utility: add filters library --- Tests/CMakeLists.txt | 1 + Tests/filters.cpp | 178 ++++++++++++++++++++++++++++++++ Utility/Inc/utility_filters.hpp | 124 ++++++++++++++++++++++ Utility/Inc/utility_ifilter.hpp | 49 +++++++++ 4 files changed, 352 insertions(+) create mode 100644 Tests/filters.cpp create mode 100644 Utility/Inc/utility_filters.hpp create mode 100644 Utility/Inc/utility_ifilter.hpp diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index 745b313..7d01952 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -30,6 +30,7 @@ add_executable(tests assert.cpp enum_helpers.cpp bytes.cpp + filters.cpp ) target_link_libraries(tests diff --git a/Tests/filters.cpp b/Tests/filters.cpp new file mode 100644 index 0000000..8acd908 --- /dev/null +++ b/Tests/filters.cpp @@ -0,0 +1,178 @@ +// +// Created by erki on 12/11/22. +// + +#include + +#include "utility_filters.hpp" + +TEST_CASE("LowPassFilter works as expected.") +{ + const int threshold = 50; + Utility::LowPassFilter filter{threshold}; + + SECTION("Initial value is 0.") + { + REQUIRE(filter.currentValue() == 0); + } + + SECTION("Passing a number higher than threshold gets filtered.") + { + filter.update(60); + REQUIRE(filter.currentValue() == 0); + + SECTION("Filter updates state as expected.") + { + filter.update(40); + REQUIRE(filter.currentValue() == 40); + } + } + + SECTION("Passing a number lower than threshold passes filter.") + { + filter.update(40); + REQUIRE(filter.currentValue() == 40); + + SECTION("Filter retains state as expected.") + { + filter.update(60); + REQUIRE(filter.currentValue() == 40); + } + } +} + +TEST_CASE("HighPassFilter works as expected.") +{ + const int threshold = 50; + Utility::HighPassFilter filter{threshold}; + + SECTION("Initial value is 0.") + { + REQUIRE(filter.currentValue() == 0); + } + + SECTION("Passing a number higher than threshold passes filter.") + { + filter.update(60); + REQUIRE(filter.currentValue() == 60); + + SECTION("Filter retains state as expected.") + { + filter.update(40); + REQUIRE(filter.currentValue() == 60); + } + } + + SECTION("Passing a number lower than threshold gets filtered.") + { + filter.update(40); + REQUIRE(filter.currentValue() == 0); + + SECTION("Filter updates state as expected.") + { + filter.update(60); + REQUIRE(filter.currentValue() == 60); + } + } +} + +TEST_CASE("Two filters can be linked together.") +{ + const int threshold_low = 40; + const int threshold_high = 50; + + Utility::LowPassFilter low_pass{threshold_high}; + Utility::HighPassFilter high_pass{threshold_low}; + + high_pass.assignPrecedingFilter(low_pass); + + SECTION("Initial value is 0.") + { + REQUIRE(high_pass.currentValue() == 0); + } + + SECTION("Values outside of band are filtered out.") + { + high_pass.update(30); + REQUIRE(high_pass.currentValue() == 0); + + high_pass.update(60); + REQUIRE(high_pass.currentValue() == 0); + + SECTION("Filter updates with in-band value.") + { + high_pass.update(45); + REQUIRE(high_pass.currentValue() == 45); + } + } + + SECTION("Values inside of band are passed through.") + { + high_pass.update(44); + REQUIRE(high_pass.currentValue() == 44); + + high_pass.update(46); + REQUIRE(high_pass.currentValue() == 46); + + SECTION("Out of band values are filtered out.") + { + high_pass.update(30); + REQUIRE(high_pass.currentValue() == 46); + + high_pass.update(60); + REQUIRE(high_pass.currentValue() == 46); + } + } +} + +TEST_CASE("Median filter works as expected.") +{ + Utility::MedianFilter filter; + + SECTION("Initial value is 0.") + { + REQUIRE(filter.currentValue() == 0); + } + + SECTION("The median is filtered appropriately.") + { + std::array data = {5, 1, 4, 3, 2}; + std::array expected_median = {0, 0, 1, 3, 3}; + for (int i = 0; i < 5; i++) + { + const int x = data[i]; + filter.update(x); + REQUIRE(filter.currentValue() == expected_median[i]); + } + } +} + +TEST_CASE("Rolling average filter works as expected.") +{ + Utility::RollingAverageFilter filter{5}; + + SECTION("Initial value is 0.") + { + REQUIRE(filter.currentValue() == 0); + } + + SECTION("Value below step size is filtered out for int filter.") + { + filter.update(4); + REQUIRE(filter.currentValue() == 0); + } + + SECTION("Value is updated appropriately.") + { + filter.update(20); + REQUIRE(filter.currentValue() == 20 / 5); + + SECTION("Saturation is reached appropriately") + { + for (int i = 0; i < 5 * 2 - 1; i++) + filter.update(20); + + REQUIRE(filter.currentValue() == 20); + } + } +} diff --git a/Utility/Inc/utility_filters.hpp b/Utility/Inc/utility_filters.hpp new file mode 100644 index 0000000..d52eac8 --- /dev/null +++ b/Utility/Inc/utility_filters.hpp @@ -0,0 +1,124 @@ +// +// Created by erki on 12/11/22. +// + +#ifndef SKULLC_UTILITY_FILTERS_HPP_ +#define SKULLC_UTILITY_FILTERS_HPP_ + +#include +#include + +#include "utility_ifilter.hpp" + +namespace Utility +{ + +template +class RollingAverageFilter : public IFilter +{ +public: + RollingAverageFilter() = delete; + explicit RollingAverageFilter(const std::size_t step) + : step_size_(step) + {} + + T currentValue() const override + { + return value_; + } + +protected: + void updateImpl_(const T& data) override + { + value_ -= value_ / step_size_; + value_ += data / step_size_; + } + +private: + std::size_t step_size_; + T value_ = 0; +}; + +template +class MedianFilter : public IFilter +{ +public: + MedianFilter() + { + data_sequence_.fill(T(0)); + } + + T currentValue() const override + { + auto sorted = data_sequence_; + std::sort(sorted.begin(), sorted.end()); + + return sorted[N / 2]; + } + +protected: + void updateImpl_(const T& data) override + { + std::move(data_sequence_.begin() + 1, data_sequence_.end(), data_sequence_.begin()); + data_sequence_[N - 1] = data; + } + +private: + std::array data_sequence_; +}; + +template +class LowPassFilter : public IFilter +{ +public: + LowPassFilter() = delete; + explicit LowPassFilter(const T threshold) + : threshold_(threshold) + {} + + T currentValue() const override + { + return value_; + } + +protected: + void updateImpl_(const T& data) override + { + if (data < threshold_) + value_ = data; + } + +private: + T threshold_; + T value_ = 0; +}; + +template +class HighPassFilter : public IFilter +{ +public: + HighPassFilter() = delete; + explicit HighPassFilter(const T threshold) + : threshold_(threshold) + {} + + T currentValue() const override + { + return value_; + } + +protected: + void updateImpl_(const T& data) override + { + if (data > threshold_) + value_ = data; + } + +private: + T threshold_; + T value_ = 0; +}; + +}// namespace Utility + +#endif//SKULLC_UTILITY_FILTERS_HPP_ diff --git a/Utility/Inc/utility_ifilter.hpp b/Utility/Inc/utility_ifilter.hpp new file mode 100644 index 0000000..b321832 --- /dev/null +++ b/Utility/Inc/utility_ifilter.hpp @@ -0,0 +1,49 @@ +// +// Created by erki on 12/11/22. +// + +#ifndef SKULLC_UTILITY_IFILTER_HPP_ +#define SKULLC_UTILITY_IFILTER_HPP_ + +namespace Utility +{ + +template +class IFilter +{ +public: + virtual ~IFilter() {} + + void assignPrecedingFilter(IFilter& filter) + { + preceding_filter_ = &filter; + } + + void clearPrecedingFilter() + { + preceding_filter_ = nullptr; + } + + void update(T data) + { + if (preceding_filter_) + { + preceding_filter_->update(data); + data = preceding_filter_->currentValue(); + } + + updateImpl_(data); + } + + virtual T currentValue() const = 0; + +protected: + virtual void updateImpl_(const T& data) = 0; + +private: + IFilter* preceding_filter_ = nullptr; +}; + +}// namespace Utility + +#endif//SKULLC_UTILITY_IFILTER_HPP_