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 
Median Filter.
Definition: filters.h:318
virtual const yarp::sig::Vector & output() const
Return current filter output.
Definition: filters.h:365
yarp::sig::Vector y
Definition: filters.h:81
IIR and FIR.
Definition: filters.h:76
size_t getOrder() const
Returns the current filter order.
Definition: filters.h:352
virtual const yarp::sig::Vector & output() const
Return current filter output.
Definition: filters.h:232
int n
std::deque< std::deque< double > > uold
Definition: filters.h:321
static int v
Definition: iCub_Sim.cpp:47
yarp::sig::Vector rateLowerLim
Definition: filters.h:186
virtual const yarp::sig::Vector & output() const
Return current filter output.
Definition: filters.h:171
yarp::sig::Vector rateUpperLim
Definition: filters.h:185
virtual const yarp::sig::Vector & output() const =0
Return current filter output.
yarp::sig::Vector uD
Definition: filters.h:183
Interface for the filters implemented in iCub::crtl.
Definition: filters.h:42
Rate Limiter.
Definition: filters.h:180
yarp::sig::Vector uLim
Definition: filters.h:184
double getCutFrequency()
Retrieve the cut frequency of the filter.
Definition: filters.h:290
double getSampleTime()
Retrieve the sample time of the filter.
Definition: filters.h:296
First order low pass filter implementing the transfer function H(s) = {1}{1+ s}.
Definition: filters.h:243
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
yarp::sig::Vector a
Definition: filters.h:80
virtual void init(const yarp::sig::Vector &y0)=0
Internal state reset.
std::deque< yarp::sig::Vector > uold
Definition: filters.h:83
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)=0
Performs filtering on the actual input.
yarp::sig::Vector b
Definition: filters.h:79
virtual ~IFilter()
Destructor.
Definition: filters.h:48
virtual const yarp::sig::Vector & output() const
Return current filter output.
Definition: filters.h:309
std::deque< yarp::sig::Vector > yold
Definition: filters.h:84
yarp::sig::Vector y
Definition: filters.h:322