moving_window_filter.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_MOVING_WINDOW_FILTER_HPP_
#define ROBOTOC_MOVING_WINDOW_FILTER_HPP_
 
#include <stdexcept>
#include <iostream>
#include <cassert>
 
#include "robotoc/utils/aligned_deque.hpp"
 
namespace robotoc {
 
template <int dim>
class MovingWindowFilter {
public:
  using Vector = Eigen::Matrix<double, dim, 1>;
 
  MovingWindowFilter(const double time_length, 
                     const double min_sampling_period=0.0)
    : time_length_(time_length),
      min_sampling_period_(min_sampling_period),
      last_sampling_time_(0.0),
      time_(),
      data_(),
      average_(Vector::Zero()) {
    if (time_length <= 0.0) {
      throw std::out_of_range("[MovingWindowFilter] invalid argument: 'time_length' must be positive!");
    }
    if (min_sampling_period < 0) {
      throw std::out_of_range("[MovingWindowFilter] invalid argument: 'min_sampling_period' must be non-negative!");
    }
  }
 
  MovingWindowFilter() 
    : time_length_(0),
      min_sampling_period_(0.0),
      last_sampling_time_(0.0),
      time_(),
      data_(),
      average_(Vector::Zero()) {
  }
 
  ~MovingWindowFilter() {}
 
  MovingWindowFilter(const MovingWindowFilter&) = default;
 
  MovingWindowFilter& operator=(const MovingWindowFilter&) = default;
 
  MovingWindowFilter(MovingWindowFilter&&) noexcept = default;
 
  MovingWindowFilter& operator=(MovingWindowFilter&&) noexcept = default;
 
  void setParameters(const double time_length, 
                     const double min_sampling_period=0.0) {
    if (time_length <= 0.0) {
      throw std::out_of_range("[MovingWindowFilter] invalid argument: 'time_length' must be positive!");
    }
    if (min_sampling_period < 0) {
      throw std::out_of_range("[MovingWindowFilter] invalid argument: 'min_sampling_period' must be non-negative!");
    }
    time_length_ = time_length;
    min_sampling_period_ = min_sampling_period;
  }
 
  void clear() {
    last_sampling_time_ = 0.0;
    time_.clear();
    data_.clear();
    average_.setZero();
  }
 
  void push_back(const double t, const Vector& data) {
    if (time_.empty()) {
      time_.push_back(t);
      data_.push_back(data);
      average_ = data;
      last_sampling_time_ = t;
    }
    else {
      if (t - last_sampling_time_ >= min_sampling_period_) {
        time_.push_back(t);
        data_.push_back(data);
        while (t - time_.front() > time_length_) {
          time_.pop_front();
          data_.pop_front();
        }
        average_.setZero();
        for (const auto& e : data_) { 
          average_.noalias() += e;
        }
        average_.array() /= data_.size();
        last_sampling_time_ = t;
      }
    }
  }
 
  int size() const {
    return data_.size();
  }
 
  const Vector& average() const {
    return average_;
  }
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
private:
  double time_length_, min_sampling_period_, last_sampling_time_;
  std::deque<double> time_;
  aligned_deque<Vector> data_;
  Vector average_;
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_MOVING_WINDOW_FILTER_HPP_