diff --git a/src/shared/algorithms/Filters/MedFilter.h b/src/shared/algorithms/Filters/MedFilter.h new file mode 100644 index 0000000000000000000000000000000000000000..c4dca2a999240a0e324683a3f8d8ca31c50eb99a --- /dev/null +++ b/src/shared/algorithms/Filters/MedFilter.h @@ -0,0 +1,94 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <array> + +namespace Boardcore +{ + +/** + * @brief Implementation of a Median Filter + * + * @tparam windowSize The size of the window + * @note WARNING: should be noted that the first windowSize - 1 outputs will be + * wrong because the window is not full yet (consider using a Shadow Mode) + */ +template <size_t windowSize> +class MedFilter +{ + // windowSize must be odd + static_assert(windowSize % 2 == 1, "windowSize must be odd"); + +public: + /** + * @brief Construct a new Median Filter object + * + * @note WARNING: Initialize output at 0 at first + */ + explicit MedFilter() : window({}) {} + + /** + * @brief Filter the input + * + * @param input The input to filter + * @note WARNING: should be noted that the first windowSize - 1 outputs will + * be wrong because the window is not full yet (consider using a Shadow + * Mode) + */ + float filter(float input) + { + slideWindow(input); + return median(); + } + +private: + /** + * @brief Slide the window by one position to the left and add the new input + * to the right + * + * @param input The input to add to the window + */ + void slideWindow(float input) + { + for (size_t i = 0; i < windowSize - 1; i++) + { + window[i] = window[i + 1]; + } + window[windowSize - 1] = input; + } + + /** + * @brief Calculate the median of the window + */ + float median() + { + std::array<float, windowSize> sortedWindow = window; + std::sort(sortedWindow.begin(), sortedWindow.end()); + return sortedWindow[windowSize / 2]; + } + + std::array<float, windowSize> window; +}; + +} // namespace Boardcore