Merge pull request #1050 from YoheiKakiuchi/up_iirfilt

[TorqueFilter, IIRFilter.h] add getParameter and reset method
fkanehiro · Oct 22, 2016 · 5fd376d · 5fd376d
2 parents 28793f6 + 3a7f667
commit 5fd376d
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 2 deletions.
diff --git a/rtc/TorqueFilter/IIRFilter.cpp b/rtc/TorqueFilter/IIRFilter.cpp
@@ -29,7 +29,7 @@ IIRFilter::IIRFilter(const std::string& error_prefix) :
  m_error_prefix = error_prefix;
 }
 
-bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double> B) {
+bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double>& B) {
  m_dimension = dim;
 
  // init coefficients
@@ -61,6 +61,27 @@ bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double
  return true;
 }
 
+void IIRFilter::getParameter(int &dim, std::vector<double>& A, std::vector<double>& B)
+{
+ dim = m_dimension;
+ B.resize(m_ff_coefficients.size());
+ std::copy(m_ff_coefficients.begin(), m_ff_coefficients.end(), B.begin());
+ A.resize(0);
+ for(std::vector<double>::iterator it = m_fb_coefficients.begin();
+ it != m_fb_coefficients.end(); it++) {
+ if (it == m_fb_coefficients.begin()) {
+ A.push_back(*it);
+ } else {
+ A.push_back(- *it);
+ }
+ }
+}
+
+void IIRFilter::reset(double initial_input)
+{
+ m_previous_values.assign(m_dimension, initial_input);
+}
+
 double IIRFilter::passFilter(double input)
 {
  if (! m_initialized) {

diff --git a/rtc/TorqueFilter/IIRFilter.h b/rtc/TorqueFilter/IIRFilter.h
@@ -49,7 +49,15 @@ class IIRFilter
  butterworth filter (dimension = 2, cutoff_freq = 8Hz)
  [B, A] = butter(2, 2 * 0.004 * 8) ;;; dimension=2, 2 * dt * cutoff_freq
  */
- bool setParameter(int dim, std::vector<double>& A, std::vector<double> B);
+ bool setParameter(int dim, std::vector<double>& A, std::vector<double>& B);
+
+ /**
+ */
+ void getParameter(int &dim, std::vector<double>&A, std::vector<double>& B);
+
+ /**
+ */
+ void reset(double initial_input = 0.0);
 
  /**
  \brief Execute filtering, this method will be obsolated