ButterworthFilter.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package ArmarX
19  * @author Mirko Waechter( mirko.waechter at kit dot edu)
20  * @date 2016
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "ButterworthFilter.h"
26 
27 #include <cmath>
28 
31 
32 namespace armarx
33 {
35  {
36  }
37 
38  void
40  int sampleRate,
41  PassType filterPassType,
42  double resonance)
43  {
44  this->resonance = resonance;
45  this->frequency = frequency;
46  this->sampleRate = sampleRate;
47  this->filterPassType = filterPassType;
48  switch (filterPassType)
49  {
50  case Lowpass:
51  c = 1.0 / tan(M_PI * frequency / sampleRate);
52  a1 = 1.0 / (1.0 + resonance * c + c * c);
53  a2 = 2.0 * a1;
54  a3 = a1;
55  b1 = 2.0 * (1.0 - c * c) * a1;
56  b2 = (1.0 - resonance * c + c * c) * a1;
57  break;
58  case Highpass:
59  c = tan(M_PI * frequency / sampleRate);
60  a1 = 1.0 / (1.0 + resonance * c + c * c);
61  a2 = -2.0 * a1;
62  a3 = a1;
63  b1 = 2.0 * (c * c - 1.0) * a1;
64  b2 = (1.0 - resonance * c + c * c) * a1;
65  break;
66  default:
67  ARMARX_INFO << "Unknown pass type";
68  }
69  }
70 
72  int sampleRate,
73  armarx::PassType filterPassType,
74  double resonance)
75  {
76  reset(frequency, sampleRate, filterPassType, resonance);
77  }
78 
79  void
81  {
82  for (auto& v : inputHistory)
83  {
84  v = value;
85  }
86  for (auto& v : outputHistory)
87  {
88  v = value;
89  }
90  }
91 
92  armarx::ParameterTypeList
94  {
95  ParameterTypeList result;
96  result.push_back(VariantType::Int);
97  result.push_back(VariantType::Long);
98  result.push_back(VariantType::Float);
99  result.push_back(VariantType::Double);
100  return result;
101  }
102 
103  void
106  const Ice::Current&)
107  {
108  //no lock required since no member data is changed
109  VariantPtr var = VariantPtr::dynamicCast(value);
110  if (!var->getInitialized())
111  {
112  return;
113  }
114 
115  double newInput = 0.0;
116  auto type = var->getType();
117  if (type == VariantType::Float)
118  {
119  newInput = static_cast<double>(var->getFloat());
120  }
121  else if (type == VariantType::Double)
122  {
123  newInput = var->getDouble();
124  }
125  else if (type == VariantType::Int)
126  {
127  newInput = static_cast<double>(var->getInt());
128  }
129  else if (type == VariantType::Long)
130  {
131  newInput = static_cast<double>(var->getLong());
132  }
133  update(newInput);
134  }
135 
136  void
138  {
139  std::unique_lock lock(historyMutex);
140  ARMARX_CHECK_EXPRESSION(!std::isnan(newInput));
141  double newOutput = a1 * newInput + a2 * this->inputHistory[0] + a3 * this->inputHistory[1] -
142  b1 * this->outputHistory[0] - b2 * this->outputHistory[1];
143  ARMARX_CHECK_EXPRESSION(!std::isnan(newOutput));
144  this->inputHistory[1] = this->inputHistory[0];
145  this->inputHistory[0] = newInput;
146 
147  this->outputHistory[2] = this->outputHistory[1];
148  this->outputHistory[1] = this->outputHistory[0];
149  this->outputHistory[0] = newOutput;
150  }
151 
154  {
155  std::unique_lock lock(historyMutex);
156 
157  return new Variant(outputHistory[0]);
158  }
159 
160  double
162  {
163  std::unique_lock lock(historyMutex);
164  return outputHistory[0];
165  }
166 
169  {
170  std::unique_lock lock(historyMutex);
171 
172  return new Variant(outputHistory[0]);
173  }
174 
175  StringFloatDictionary
177  {
178  StringFloatDictionary props;
179  props["resonance"] = resonance;
180  props["frequency"] = frequency;
181  props["sampleRate"] = sampleRate;
182  props["filterPassType"] = (int)(filterPassType);
183  return props;
184  }
185 
186  void
187  armarx::filters::ButterworthFilter::setProperties(const StringFloatDictionary& newValues,
188  const Ice::Current&)
189  {
190  auto it = newValues.find("resonance");
191  if (it != newValues.end())
192  {
193  resonance = it->second;
194  }
195 
196  it = newValues.find("frequency");
197  if (it != newValues.end())
198  {
199  frequency = it->second;
200  }
201 
202  it = newValues.find("sampleRate");
203  if (it != newValues.end())
204  {
205  sampleRate = it->second;
206  }
207 
208  it = newValues.find("filterPassType");
209  if (it != newValues.end())
210  {
211  filterPassType = static_cast<PassType>(it->second);
212  }
213  reset(frequency, sampleRate, filterPassType, resonance);
214  }
215 } // namespace armarx
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:919
armarx::filters::ButterworthFilter::update
void update(Ice::Long, const VariantBasePtr &value, const Ice::Current &=Ice::emptyCurrent) override
Definition: ButterworthFilter.cpp:104
armarx::filters::ButterworthFilter::setProperties
void setProperties(const StringFloatDictionary &values, const Ice::Current &) override
Definition: ButterworthFilter.cpp:187
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
ButterworthFilter.h
IceInternal::Handle<::armarx::VariantBase >
armarx::filters::ButterworthFilter::getProperties
StringFloatDictionary getProperties(const Ice::Current &) const override
Definition: ButterworthFilter.cpp:176
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
armarx::filters::ButterworthFilter::calculate
VariantBasePtr calculate(const Ice::Current &=Ice::emptyCurrent) const override
Definition: ButterworthFilter.cpp:168
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:855
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:918
armarx::filters::ButterworthFilter::reset
void reset(double frequency, int sampleRate, PassType passType, double resonance)
Definition: ButterworthFilter.cpp:39
armarx::filters::ButterworthFilter::getValue
VariantBasePtr getValue(const Ice::Current &=Ice::emptyCurrent) const override
Definition: ButterworthFilter.cpp:153
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:68
ExpressionException.h
armarx::filters::ButterworthFilter::ButterworthFilter
ButterworthFilter()
Definition: ButterworthFilter.cpp:34
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::VariantType::Int
const VariantTypeId Int
Definition: Variant.h:917
Logging.h
armarx::filters::ButterworthFilter::setInitialValue
void setInitialValue(double value)
Definition: ButterworthFilter.cpp:80
armarx::filters::ButterworthFilter::getRawValue
double getRawValue() const
Definition: ButterworthFilter.cpp:161
armarx::filters::ButterworthFilter::getSupportedTypes
ParameterTypeList getSupportedTypes(const Ice::Current &=Ice::emptyCurrent) const override
This filter supports: Int, Long, Float, Double.
Definition: ButterworthFilter.cpp:93
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27