LowPassCompose.h

Go to the documentation of this file.

#pragma once
 
#include <mc_rtc/logging.h>
#include <algorithm>
 
namespace mc_filter
{
 
template<typename T>
struct LowPassCompose
{
  LowPassCompose(double dt, double period = 0)
  {
    cutoffPeriod(period);
    dt_ = dt;
    reset(T::Zero());
  }
 
  double cutoffPeriod() const { return cutoffPeriod_; }
 
  void cutoffPeriod(double period)
  {
    if(period < 2 * dt_)
    {
      mc_rtc::log::warning("Time constant must be at least twice the timestep (Nyquist–Shannon sampling theorem)");
      period = 2 * dt_;
    }
    cutoffPeriod_ = period;
  }
 
  void reset(const T & value)
  {
    eval_ = value;
    eval_hp_ = eval_;
    eval_lp_ = eval_;
  }
 
  void update(const T & newValue_hp, const T & newValue_lp)
  {
    double x = (cutoffPeriod_ <= dt_) ? 1. : dt_ / cutoffPeriod_;
 
    // Update the low pass filter
    eval_lp_ = x * newValue_lp + (1. - x) * eval_lp_;
 
    // Update the high pass filter output
    T eval_hp = (newValue_hp - eval_hp_);
 
    // Summation of both components
    eval_ = eval_hp + eval_lp_;
 
    // Update high pass filter state
    eval_hp_ = (x * newValue_hp + (1. - x) * eval_hp_);
 
    // Use for logs
    value_hp_ = newValue_hp;
    value_lp_ = newValue_lp;
  }
 
  const T & eval() const { return eval_; }
 
  const T & input_lp() const { return value_lp_; }
  const T & input_hp() const { return value_hp_; }
 
  double dt() const { return dt_; }
 
  void dt(double dt)
  {
    dt_ = dt;
    cutoffPeriod(cutoffPeriod_);
  }
 
private:
  T eval_;
  T eval_lp_;
  T eval_hp_;
  T value_hp_;
  T value_lp_;
  double cutoffPeriod_ = 0.;
 
protected:
  double dt_ = 0.005; // [s]
};
 
} // namespace mc_filter