iCub-main
filters.cpp
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 
11 #include <cmath>
12 #include <limits>
13 #include <algorithm>
14 
15 #include <yarp/os/Log.h>
16 #include <yarp/math/Math.h>
17 #include <iCub/ctrl/filters.h>
18 
19 using namespace std;
20 using namespace yarp::sig;
21 using namespace yarp::math;
22 using namespace iCub::ctrl;
23 
24 
25 /***************************************************************************/
26 Filter::Filter(const Vector &num, const Vector &den, const Vector &y0)
27 {
28  b=num;
29  a=den;
30 
31  m=b.length(); n=a.length();
32  yAssert((m>0)&&(n>0));
33 
34  uold.insert(uold.begin(),m-1,zeros((int)y0.length()));
35  yold.insert(yold.begin(),n-1,zeros((int)y0.length()));
36 
37  init(y0);
38 }
39 
40 
41 /***************************************************************************/
42 void Filter::init(const Vector &y0)
43 {
44  // take the last input
45  // as guess for the next input
46  if (uold.size()>0)
47  init(y0,uold[0]);
48  else // otherwise use zero
49  init(y0,zeros((int)y0.length()));
50 }
51 
52 
53 /***************************************************************************/
54 void Filter::init(const Vector &y0, const Vector &u0)
55 {
56  Vector u_init(y0.length(),0.0);
57  Vector y_init=y0;
58  y=y0;
59 
60  double sum_b=0.0;
61  for (size_t i=0; i<b.length(); i++)
62  sum_b+=b[i];
63 
64  double sum_a=0.0;
65  for (size_t i=0; i<a.length(); i++)
66  sum_a+=a[i];
67 
68  // if filter DC gain is not zero
69  if (fabs(sum_b)>std::numeric_limits<double>::epsilon())
70  u_init=(sum_a/sum_b)*y0;
71  else
72  {
73  // if filter gain is zero then you need to know in advance what
74  // the next input is going to be for initializing (that is u0)
75  // Note that, unless y0=0, the filter output is not going to be stable
76  u_init=u0;
77  if (fabs(sum_a-a[0])>std::numeric_limits<double>::epsilon())
78  y_init=a[0]/(a[0]-sum_a)*y;
79  // if sum_a==a[0] then the filter can only be initialized to zero
80  }
81 
82  for (size_t i=0; i<yold.size(); i++)
83  yold[i]=y_init;
84 
85  for (size_t i=0; i<uold.size(); i++)
86  uold[i]=u_init;
87 }
88 
89 
90 /***************************************************************************/
91 void Filter::getCoeffs(Vector &num, Vector &den)
92 {
93  num=b;
94  den=a;
95 }
96 
97 
98 /***************************************************************************/
99 void Filter::setCoeffs(const Vector &num, const Vector &den)
100 {
101  b=num;
102  a=den;
103 
104  uold.clear();
105  yold.clear();
106 
107  m=b.length(); n=a.length();
108  yAssert((m>0)&&(n>0));
109 
110  uold.insert(uold.begin(),m-1,zeros((int)y.length()));
111  yold.insert(yold.begin(),n-1,zeros((int)y.length()));
112 
113  init(y);
114 }
115 
116 
117 /***************************************************************************/
118 bool Filter::adjustCoeffs(const Vector &num, const Vector &den)
119 {
120  if ((num.length()==b.length()) && (den.length()==a.length()))
121  {
122  b=num;
123  a=den;
124  return true;
125  }
126  else
127  return false;
128 }
129 
130 
131 /***************************************************************************/
132 void Filter::getStates(deque<Vector> &u, deque<Vector> &y)
133 {
134  u=uold;
135  y=yold;
136 }
137 
138 
139 /***************************************************************************/
140 const Vector& Filter::filt(const Vector &u)
141 {
142  yAssert(y.length()==u.length());
143  for (size_t j=0; j<y.length(); j++)
144  y[j]=b[0]*u[j];
145 
146  for (size_t i=1; i<m; i++)
147  for (size_t j=0; j<y.length(); j++)
148  y[j]+=b[i]*uold[i-1][j];
149 
150  for (size_t i=1; i<n; i++)
151  for (size_t j=0; j<y.length(); j++)
152  y[j]-=a[i]*yold[i-1][j];
153 
154  for (size_t j=0; j<y.length(); j++)
155  y[j]/=a[0];
156 
157  uold.push_front(u);
158  uold.pop_back();
159 
160  yold.push_front(y);
161  yold.pop_back();
162 
163  return y;
164 }
165 
166 
167 /**********************************************************************/
168 RateLimiter::RateLimiter(const Vector &rL, const Vector &rU) :
169  rateLowerLim(rL), rateUpperLim(rU)
170 {
171  size_t nL=rateLowerLim.length();
172  size_t nU=rateUpperLim.length();
173  n=std::min(nL,nU);
174 }
175 
176 
177 /**********************************************************************/
178 void RateLimiter::init(const Vector &u0)
179 {
180  uLim=u0;
181 }
182 
183 
184 /**********************************************************************/
185 void RateLimiter::getLimits(Vector &rL, Vector &rU)
186 {
187  rL=rateLowerLim;
188  rU=rateUpperLim;
189 }
190 
191 
192 /**********************************************************************/
193 void RateLimiter::setLimits(const Vector &rL, const Vector &rU)
194 {
195  rateLowerLim=rL;
196  rateUpperLim=rU;
197 }
198 
199 
200 /**********************************************************************/
201 const Vector& RateLimiter::filt(const Vector &u)
202 {
203  uD=u-uLim;
204  for (size_t i=0; i<n; i++)
205  {
206  if (uD[i]>rateUpperLim[i])
207  uD[i]=rateUpperLim[i];
208  else if (uD[i]<rateLowerLim[i])
209  uD[i]=rateLowerLim[i];
210  }
211 
212  uLim+=uD;
213  return uLim;
214 }
215 
216 
217 /**********************************************************************/
218 FirstOrderLowPassFilter::FirstOrderLowPassFilter(const double cutFrequency,
219  const double sampleTime,
220  const Vector &y0)
221 {
222  fc=cutFrequency;
223  Ts=sampleTime;
224  y=y0;
225  filter=NULL;
226  computeCoeff();
227 }
228 
229 
230 /**********************************************************************/
232 {
233  delete filter;
234 }
235 
236 
237 /***************************************************************************/
238 void FirstOrderLowPassFilter::init(const Vector &y0)
239 {
240  if (filter!=NULL)
241  filter->init(y0);
242 }
243 
244 
245 /**********************************************************************/
246 bool FirstOrderLowPassFilter::setCutFrequency(const double cutFrequency)
247 {
248  if (cutFrequency<=0.0)
249  return false;
250 
251  fc=cutFrequency;
252  computeCoeff();
253 
254  return true;
255 }
256 
257 
258 /**********************************************************************/
259 bool FirstOrderLowPassFilter::setSampleTime(const double sampleTime)
260 {
261  if (sampleTime<=0.0)
262  return false;
263 
264  Ts=sampleTime;
265  computeCoeff();
266 
267  return true;
268 }
269 
270 
271 /**********************************************************************/
272 const Vector& FirstOrderLowPassFilter::filt(const Vector &u)
273 {
274  if (filter!=NULL)
275  y=filter->filt(u);
276 
277  return y;
278 }
279 
280 
281 /**********************************************************************/
283 {
284  double tau=1.0/(2.0*M_PI*fc);
285  Vector num=cat(Ts,Ts);
286  Vector den=cat(2.0*tau+Ts,Ts-2.0*tau);
287 
288  if (filter!=NULL)
289  filter->adjustCoeffs(num,den);
290  else
291  filter=new Filter(num,den,y);
292 }
293 
294 
295 /***************************************************************************/
296 MedianFilter::MedianFilter(const size_t n, const Vector &y0)
297 {
298  this->n=n;
299  init(y0);
300 }
301 
302 
303 /***************************************************************************/
304 void MedianFilter::init(const Vector &y0)
305 {
306  yAssert(y0.length()>0);
307  y=y0;
308  m=y.length();
309  uold.assign(m,deque<double>());
310 }
311 
312 
313 
314 /***************************************************************************/
315 void MedianFilter::setOrder(const size_t n)
316 {
317  this->n=n;
318  init(y);
319 }
320 
321 
322 /***************************************************************************/
323 double MedianFilter::median(deque<double>& v)
324 {
325  size_t L=v.size()>>1;
326  nth_element(v.begin(),v.begin()+L,v.end());
327  if (v.size()&0x01)
328  return v[L];
329  else
330  {
331  nth_element(v.begin(),v.begin()+L-1,v.end());
332  return 0.5*(v[L]+v[L-1]);
333  }
334 }
335 
336 
337 /***************************************************************************/
338 const Vector& MedianFilter::filt(const Vector &u)
339 {
340  yAssert(y.length()==u.length());
341  for (size_t i=0; i<m; i++)
342  uold[i].push_front(u[i]);
343 
344  if (uold[0].size()>n)
345  {
346  for (size_t i=0; i<m; i++)
347  {
348  deque<double> tmp=uold[i];
349  y[i]=median(tmp);
350  uold[i].pop_back();
351  }
352  }
353 
354  return y;
355 }
356 
357 
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
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::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::MedianFilter::init
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
Definition: filters.cpp:304
iCub::ctrl::MedianFilter::m
size_t m
Definition: filters.h:324
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
iCub::ctrl::FirstOrderLowPassFilter::computeCoeff
void computeCoeff()
Definition: filters.cpp:282
iCub::ctrl::RateLimiter::uLim
yarp::sig::Vector uLim
Definition: filters.h:184
iCub::ctrl::FirstOrderLowPassFilter::Ts
double Ts
Definition: filters.h:248
iCub::ctrl::FirstOrderLowPassFilter::init
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
filters.h
iCub::ctrl::MedianFilter::median
double median(std::deque< double > &v)
Definition: filters.cpp:323
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::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
Definition: adaptWinPolyEstimator.h:37
iCub::ctrl::RateLimiter::rateLowerLim
yarp::sig::Vector rateLowerLim
Definition: filters.h:186
n
int n
Definition: debugFunctions.cpp:58
y
y
Definition: show_eyes_axes.m:21
iCub::ctrl::MedianFilter::uold
std::deque< std::deque< double > > uold
Definition: filters.h:321
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::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::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
zeros
zeros(2, 2) eye(2
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
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::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