186 lines
6.6 KiB
C++
186 lines
6.6 KiB
C++
#pragma once
|
|
|
|
#include <stdint.h>
|
|
#include <AH/STL/limits>
|
|
#include <AH/STL/type_traits>
|
|
|
|
#include <AH/Settings/NamespaceSettings.hpp>
|
|
|
|
BEGIN_AH_NAMESPACE
|
|
|
|
/**
|
|
* @brief Exponential moving average filter.
|
|
*
|
|
* Fast integer EMA implementation where the weight factor is a power of two.
|
|
*
|
|
* Difference equation: @f$ y[n] = \alpha·x[n]+(1-\alpha)·y[n-1] @f$
|
|
* where @f$ \alpha = \left(\frac{1}{2}\right)^{K} @f$, @f$ x @f$ is the
|
|
* input sequence, and @f$ y @f$ is the output sequence.
|
|
*
|
|
* [An in-depth explanation of the EMA filter](https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Exponential%20Moving%20Average/)
|
|
*
|
|
* @tparam K
|
|
* The amount of bits to shift by. This determines the location
|
|
* of the pole in the EMA transfer function, and therefore the
|
|
* cut-off frequency.
|
|
* The higher this number, the more filtering takes place.
|
|
* The pole location is @f$ 1 - 2^{-K} @f$.
|
|
* @tparam input_t
|
|
* The integer type to use for the input and output of the filter.
|
|
* Can be signed or unsigned.
|
|
* @tparam state_t
|
|
* The unsigned integer type to use for the internal state of the
|
|
* filter. A fixed-point representation with @f$ K @f$ fractional
|
|
* bits is used, so this type should be at least @f$ M + K @f$ bits
|
|
* wide, where @f$ M @f$ is the maximum number of bits of the input.
|
|
*
|
|
* Some examples of different combinations of template parameters:
|
|
*
|
|
* 1. Filtering the result of `analogRead`: analogRead returns an integer
|
|
* between 0 and 1023, which can be represented using 10 bits, so
|
|
* @f$ M = 10 @f$. If `input_t` and `output_t` are both `uint16_t`,
|
|
* the maximum shift factor `K` is @f$ 16 - M = 6 @f$. If `state_t`
|
|
* is increased to `uint32_t`, the maximum shift factor `K` is
|
|
* @f$ 32 - M = 22 @f$.
|
|
* 2. Filtering a signed integer between -32768 and 32767: this can be
|
|
* represented using a 16-bit signed integer, so `input_t` is `int16_t`,
|
|
* and @f$ M = 16 @f$. (2¹⁵ = 32768)
|
|
* Let's say the shift factor `K` is 1, then the minimum width of
|
|
* `state_t` should be @f$ M + K = 17 @f$ bits, so `uint32_t` would be
|
|
* a sensible choice.
|
|
*
|
|
* @ingroup AH_Filters
|
|
*/
|
|
template <uint8_t K,
|
|
class input_t = uint_fast16_t,
|
|
class state_t = typename std::make_unsigned<input_t>::type>
|
|
class EMA {
|
|
public:
|
|
/// Constructor: initialize filter to zero or optional given value.
|
|
EMA(input_t initial = input_t{0}) { reset(initial); }
|
|
|
|
/**
|
|
* @brief Reset the filter to the given value.
|
|
*
|
|
* @param value
|
|
* The value to reset the filter state to.
|
|
*/
|
|
void reset(input_t value = input_t(0)) {
|
|
state_t value_s = static_cast<state_t>(value);
|
|
state = zero + (value_s << K) - value_s;
|
|
}
|
|
|
|
/**
|
|
* @brief Filter the input: Given @f$ x[n] @f$, calculate @f$ y[n] @f$.
|
|
*
|
|
* @param input
|
|
* The new raw input value.
|
|
* @return The new filtered output value.
|
|
*/
|
|
input_t filter(input_t input) {
|
|
state += static_cast<state_t>(input);
|
|
state_t output = (state + half) >> K;
|
|
output -= zero >> K;
|
|
state -= output;
|
|
return static_cast<input_t>(output);
|
|
}
|
|
|
|
/// @copydoc EMA::filter(input_t)
|
|
input_t operator()(input_t input) {
|
|
return filter(input);
|
|
}
|
|
|
|
constexpr static state_t
|
|
max_state = std::numeric_limits<state_t>::max(),
|
|
half_state = max_state / 2 + 1,
|
|
zero = std::is_unsigned<input_t>::value ? state_t{0} : half_state,
|
|
half = K > 0 ? state_t{1} << (K - 1) : state_t{0};
|
|
|
|
static_assert(std::is_unsigned<state_t>::value,
|
|
"state type should be unsigned");
|
|
|
|
static_assert(max_state >= std::numeric_limits<input_t>::max(),
|
|
"state type cannot be narrower than input type");
|
|
|
|
/// Verify the input range to make sure it's compatible with the shift
|
|
/// factor and the width of the state type.
|
|
///
|
|
/// Examples:
|
|
/// ~~~cpp
|
|
/// EMA<5, int_fast16_t, uint_fast16_t> filter;
|
|
/// static_assert(filter.supports_range(-1024, 1023),
|
|
/// "use a wider state or input type, or a smaller shift factor");
|
|
/// ~~~
|
|
/// ~~~cpp
|
|
/// EMA<5, uint_fast16_t, uint_fast16_t> filter;
|
|
/// static_assert(filter.supports_range(0u, 2047u),
|
|
/// "use a wider state or input type, or a smaller shift factor");
|
|
/// ~~~
|
|
template <class T>
|
|
constexpr static bool supports_range(T min, T max) {
|
|
using sstate_t = typename std::make_signed<state_t>::type;
|
|
return min <= max &&
|
|
min >= std::numeric_limits<input_t>::min() &&
|
|
max <= std::numeric_limits<input_t>::max() &&
|
|
(std::is_unsigned<input_t>::value
|
|
? state_t(max) <= (max_state >> K)
|
|
: min >= -static_cast<sstate_t>(max_state >> (K + 1)) - 1 &&
|
|
max <= static_cast<sstate_t>(max_state >> (K + 1)));
|
|
}
|
|
|
|
private:
|
|
state_t state;
|
|
};
|
|
|
|
// -------------------------------------------------------------------------- //
|
|
|
|
/**
|
|
* @brief A class for single-pole infinite impulse response filters
|
|
* or exponential moving average filters.
|
|
*
|
|
* This version uses floating point maths.
|
|
*
|
|
* Difference equation: @f$ y[n] = \alpha·x[n]+(1-\alpha)·y[n-1] @f$
|
|
* @f$ x @f$ is the input sequence, and @f$ y @f$ is the output sequence.
|
|
*
|
|
* [An in-depth explanation of the EMA filter]
|
|
* (https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Exponential%20Moving%20Average/)
|
|
*
|
|
* @ingroup AH_Filters
|
|
*/
|
|
class EMA_f {
|
|
public:
|
|
/**
|
|
* @brief Create an exponential moving average filter with a pole at the
|
|
* given location.
|
|
*
|
|
* @param pole
|
|
* The pole of the filter (@f$1-\alpha@f$).
|
|
* Should be a value in the range
|
|
* @f$ \left[0,1\right) @f$.
|
|
* Zero means no filtering, and closer to one means more filtering.
|
|
*/
|
|
EMA_f(float pole) : alpha(1 - pole) {}
|
|
|
|
/**
|
|
* @brief Filter the input: Given @f$ x[n] @f$, calculate @f$ y[n] @f$.
|
|
*
|
|
* @param value
|
|
* The new raw input value.
|
|
* @return The new filtered output value.
|
|
*/
|
|
float filter(float value) {
|
|
filtered += (value - filtered) * alpha;
|
|
return filtered;
|
|
}
|
|
|
|
/// @copydoc filter(float)
|
|
float operator()(float value) { return filter(value); }
|
|
|
|
private:
|
|
float alpha;
|
|
float filtered = 0;
|
|
};
|
|
|
|
END_AH_NAMESPACE
|