EMA.hpp

Go to the documentation of this file.

/* ✔ */
 
#pragma once
 
#include <AH/Settings/Warnings.hpp>
AH_DIAGNOSTIC_WERROR() // Enable errors on warnings
 
#include <stdint.h>
#include <AH/STL/limits>
#include <AH/STL/type_traits>
 
template <uint8_t K,
          class input_t = uint_fast16_t,
          class state_t = typename std::make_unsigned<input_t>::type>
class EMA {
  public:
    EMA(input_t initial = input_t(0))
      : state(zero + (state_t(initial) << K) - initial) {}
 
    void reset(input_t value = input_t(0)) {
        state = zero + (state_t(value) << K) - value;
    }
 
    input_t filter(input_t input) {
      state         += state_t(input);
      state_t output = (state + half) >> K;
      output        -= zero >> K;
      state         -= output;
      return input_t(output);
    }
 
    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");               
 
    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 >= -sstate_t(max_state >> (K + 1)) - 1 &&  
                 max <= sstate_t(max_state >> (K + 1)));
    }
 
  private:
    state_t state;
};
 
// -------------------------------------------------------------------------- //
 
class EMA_f {
  public:
    EMA_f(float pole) : alpha(1 - pole) {}
 
    float filter(float value) {
        filtered += (value - filtered) * alpha;
        return filtered;
    }
 
    float operator()(float value) { return filter(value); }
 
  private:
    float alpha;
    float filtered = 0;
};
 
AH_DIAGNOSTIC_POP()