iCub-main
filters.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
22 #ifndef __FILTERS_H__
23 #define __FILTERS_H__
24 
25 #include <deque>
26 
27 #include <yarp/sig/Vector.h>
28 #include <iCub/ctrl/math.h>
29 
30 
31 namespace iCub
32 {
33 
34 namespace ctrl
35 {
36 
42 class IFilter
43 {
44 public:
48  virtual ~IFilter() { }
49 
54  virtual void init(const yarp::sig::Vector& y0) = 0;
55 
61  virtual const yarp::sig::Vector& filt(const yarp::sig::Vector& u) = 0;
62 
67  virtual const yarp::sig::Vector& output() const = 0;
68 };
69 
70 
76 class Filter : public IFilter
77 {
78 protected:
79  yarp::sig::Vector b;
80  yarp::sig::Vector a;
81  yarp::sig::Vector y;
82 
83  std::deque<yarp::sig::Vector> uold;
84  std::deque<yarp::sig::Vector> yold;
85  size_t n;
86  size_t m;
87 
88 public:
99  Filter(const yarp::sig::Vector &num, const yarp::sig::Vector &den,
100  const yarp::sig::Vector &y0=yarp::sig::Vector(1,0.0));
101 
106  virtual void init(const yarp::sig::Vector &y0);
107 
115  virtual void init(const yarp::sig::Vector &y0, const yarp::sig::Vector &u0);
116 
124  void getCoeffs(yarp::sig::Vector &num, yarp::sig::Vector &den);
125 
136  void setCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den);
137 
151  bool adjustCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den);
152 
158  void getStates(std::deque<yarp::sig::Vector> &u, std::deque<yarp::sig::Vector> &y);
159 
165  virtual const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
166 
171  virtual const yarp::sig::Vector& output() const { return y; }
172 };
173 
174 
180 class RateLimiter : public IFilter
181 {
182 protected:
183  yarp::sig::Vector uD;
184  yarp::sig::Vector uLim;
185  yarp::sig::Vector rateUpperLim;
186  yarp::sig::Vector rateLowerLim;
187 
188  size_t n;
189 
190 public:
197  RateLimiter(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU);
198 
203  virtual void init(const yarp::sig::Vector &u0);
204 
210  void getLimits(yarp::sig::Vector &rL, yarp::sig::Vector &rU);
211 
219  void setLimits(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU);
220 
226  virtual const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
227 
232  virtual const yarp::sig::Vector& output() const { return uLim; }
233 };
234 
235 
244 {
245 protected:
246  Filter *filter; // low pass filter
247  double fc; // cut frequency
248  double Ts; // sample time
249  yarp::sig::Vector y; // filter current output
250 
251  void computeCoeff();
252 
253 public:
260  FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime,
261  const yarp::sig::Vector &y0=yarp::sig::Vector(1,0.0));
262 
266  virtual ~FirstOrderLowPassFilter();
267 
272  virtual void init(const yarp::sig::Vector &y0);
273 
278  bool setCutFrequency(const double cutFrequency);
279 
284  bool setSampleTime(const double sampleTime);
285 
290  double getCutFrequency() { return fc; }
291 
296  double getSampleTime() { return Ts; }
297 
303  virtual const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
304 
309  virtual const yarp::sig::Vector& output() const { return y; }
310 };
311 
312 
318 class MedianFilter : public IFilter
319 {
320 protected:
321  std::deque<std::deque<double> > uold;
322  yarp::sig::Vector y;
323  size_t n;
324  size_t m;
325 
326  double median(std::deque<double>& v);
327 
328 public:
334  MedianFilter(const size_t n, const yarp::sig::Vector &y0=yarp::sig::Vector(1,0.0));
335 
340  virtual void init(const yarp::sig::Vector &y0);
341 
347  void setOrder(const size_t n);
348 
352  size_t getOrder() const { return n; }
353 
359  virtual const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
360 
365  virtual const yarp::sig::Vector& output() const { return y; }
366 };
367 
368 }
369 
370 }
371 
372 #endif
373 
374 
375 
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::ctrl::FirstOrderLowPassFilter::setCutFrequency
bool setCutFrequency(const double cutFrequency)
Change the cut frequency of the filter.
Definition: filters.cpp:246
iCub::ctrl::RateLimiter::setLimits
void setLimits(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU)
Sets new Rate limits.
Definition: filters.cpp:193
iCub::ctrl::MedianFilter::n
size_t n
Definition: filters.h:323
iCub::ctrl::Filter::a
yarp::sig::Vector a
Definition: filters.h:80
iCub::ctrl::IFilter::~IFilter
virtual ~IFilter()
Destructor.
Definition: filters.h:48
iCub::ctrl::MedianFilter::filt
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
Definition: filters.cpp:338
iCub::ctrl::RateLimiter::rateUpperLim
yarp::sig::Vector rateUpperLim
Definition: filters.h:185
iCub::ctrl::RateLimiter::output
virtual const yarp::sig::Vector & output() const
Return current filter output.
Definition: filters.h:232
iCub::ctrl::MedianFilter::init
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
Definition: filters.cpp:304
iCub::ctrl::FirstOrderLowPassFilter::getSampleTime
double getSampleTime()
Retrieve the sample time of the filter.
Definition: filters.h:296
iCub::ctrl::MedianFilter::m
size_t m
Definition: filters.h:324
iCub::ctrl::FirstOrderLowPassFilter::output
virtual const yarp::sig::Vector & output() const
Return current filter output.
Definition: filters.h:309
iCub::ctrl::Filter::getCoeffs
void getCoeffs(yarp::sig::Vector &num, yarp::sig::Vector &den)
Returns the current filter coefficients.
Definition: filters.cpp:91
iCub::ctrl::IFilter
Definition: filters.h:42
iCub::ctrl::FirstOrderLowPassFilter::computeCoeff
void computeCoeff()
Definition: filters.cpp:282
iCub::ctrl::Filter::yold
std::deque< yarp::sig::Vector > yold
Definition: filters.h:84
iCub::ctrl::RateLimiter::uLim
yarp::sig::Vector uLim
Definition: filters.h:184
iCub::ctrl::FirstOrderLowPassFilter::Ts
double Ts
Definition: filters.h:248
iCub::ctrl::Filter::getStates
void getStates(std::deque< yarp::sig::Vector > &u, std::deque< yarp::sig::Vector > &y)
Returns the current filter states.
Definition: filters.cpp:132
iCub::ctrl::FirstOrderLowPassFilter
Definition: filters.h:243
math.h
iCub::ctrl::FirstOrderLowPassFilter::init
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
iCub::ctrl::Filter::setCoeffs
void setCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den)
Sets new filter coefficients.
Definition: filters.cpp:99
iCub::ctrl::MedianFilter::median
double median(std::deque< double > &v)
Definition: filters.cpp:323
iCub::ctrl::Filter::y
yarp::sig::Vector y
Definition: filters.h:81
iCub::ctrl::IFilter::init
virtual void init(const yarp::sig::Vector &y0)=0
Internal state reset.
iCub::ctrl::Filter::adjustCoeffs
bool adjustCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den)
Modifies the values of existing filter coefficients without varying their lengths.
Definition: filters.cpp:118
iCub::ctrl::FirstOrderLowPassFilter::fc
double fc
Definition: filters.h:247
iCub::ctrl::MedianFilter::y
yarp::sig::Vector y
Definition: filters.h:322
iCub::ctrl::FirstOrderLowPassFilter::~FirstOrderLowPassFilter
virtual ~FirstOrderLowPassFilter()
Destructor.
Definition: filters.cpp:231
iCub::ctrl::RateLimiter::n
size_t n
Definition: filters.h:188
iCub::ctrl::IFilter::output
virtual const yarp::sig::Vector & output() const =0
Return current filter output.
iCub::ctrl::MedianFilter::output
virtual const yarp::sig::Vector & output() const
Return current filter output.
Definition: filters.h:365
iCub::ctrl::RateLimiter::filt
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Limits the input rate.
Definition: filters.cpp:201
iCub::ctrl::Filter
Definition: filters.h:76
iCub::ctrl::RateLimiter::rateLowerLim
yarp::sig::Vector rateLowerLim
Definition: filters.h:186
iCub::ctrl::FirstOrderLowPassFilter::getCutFrequency
double getCutFrequency()
Retrieve the cut frequency of the filter.
Definition: filters.h:290
iCub::ctrl::IFilter::filt
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)=0
Performs filtering on the actual input.
iCub::ctrl::Filter::b
yarp::sig::Vector b
Definition: filters.h:79
iCub::ctrl::Filter::Filter
Filter(const yarp::sig::Vector &num, const yarp::sig::Vector &den, const yarp::sig::Vector &y0=yarp::sig::Vector(1, 0.0))
Creates a filter with specified numerator and denominator coefficients.
Definition: filters.cpp:26
iCub::ctrl::MedianFilter::uold
std::deque< std::deque< double > > uold
Definition: filters.h:321
iCub::ctrl::RateLimiter
Definition: filters.h:180
iCub::ctrl::MedianFilter::MedianFilter
MedianFilter(const size_t n, const yarp::sig::Vector &y0=yarp::sig::Vector(1, 0.0))
Creates a median filter of the specified order.
Definition: filters.cpp:296
iCub::ctrl::Filter::filt
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
Definition: filters.cpp:140
iCub::ctrl::MedianFilter::getOrder
size_t getOrder() const
Returns the current filter order.
Definition: filters.h:352
iCub::ctrl::Filter::output
virtual const yarp::sig::Vector & output() const
Return current filter output.
Definition: filters.h:171
iCub::ctrl::Filter::init
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
v
static int v
Definition: iCub_Sim.cpp:42
iCub::ctrl::FirstOrderLowPassFilter::filter
Filter * filter
Definition: filters.h:246
iCub::ctrl::RateLimiter::getLimits
void getLimits(yarp::sig::Vector &rL, yarp::sig::Vector &rU)
Returns the current Rate limits.
Definition: filters.cpp:185
iCub::ctrl::RateLimiter::RateLimiter
RateLimiter(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU)
Creates a Rate Limiter which keeps the rate of the input within assigned thresholds.
Definition: filters.cpp:168
iCub::ctrl::MedianFilter::setOrder
void setOrder(const size_t n)
Sets new filter order.
Definition: filters.cpp:315
iCub::ctrl::FirstOrderLowPassFilter::y
yarp::sig::Vector y
Definition: filters.h:249
iCub::ctrl::FirstOrderLowPassFilter::setSampleTime
bool setSampleTime(const double sampleTime)
Change the sample time of the filter.
Definition: filters.cpp:259
iCub::ctrl::MedianFilter
Definition: filters.h:318
iCub::ctrl::Filter::m
size_t m
Definition: filters.h:86
iCub::ctrl::Filter::n
size_t n
Definition: filters.h:85
iCub::ctrl::RateLimiter::uD
yarp::sig::Vector uD
Definition: filters.h:183
iCub::ctrl::FirstOrderLowPassFilter::FirstOrderLowPassFilter
FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime, const yarp::sig::Vector &y0=yarp::sig::Vector(1, 0.0))
Creates a filter with specified parameters.
iCub::ctrl::Filter::uold
std::deque< yarp::sig::Vector > uold
Definition: filters.h:83
iCub::ctrl::FirstOrderLowPassFilter::filt
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
iCub::ctrl::RateLimiter::init
virtual void init(const yarp::sig::Vector &u0)
Init internal state.
Definition: filters.cpp:178