iCub-main
Loading...
Searching...
No Matches
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
19using namespace std;
20using namespace yarp::sig;
21using namespace yarp::math;
22using namespace iCub::ctrl;
23
24
25/***************************************************************************/
26Filter::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/***************************************************************************/
42void 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/***************************************************************************/
54void 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/***************************************************************************/
91void Filter::getCoeffs(Vector &num, Vector &den)
92{
93 num=b;
94 den=a;
95}
96
97
98/***************************************************************************/
99void 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/***************************************************************************/
118bool 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/***************************************************************************/
132void Filter::getStates(deque<Vector> &u, deque<Vector> &y)
133{
134 u=uold;
135 y=yold;
136}
137
138
139/***************************************************************************/
140const 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/**********************************************************************/
168RateLimiter::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/**********************************************************************/
178void RateLimiter::init(const Vector &u0)
179{
180 uLim=u0;
181}
182
183
184/**********************************************************************/
185void RateLimiter::getLimits(Vector &rL, Vector &rU)
186{
187 rL=rateLowerLim;
188 rU=rateUpperLim;
189}
190
191
192/**********************************************************************/
193void RateLimiter::setLimits(const Vector &rL, const Vector &rU)
194{
195 rateLowerLim=rL;
196 rateUpperLim=rU;
197}
198
199
200/**********************************************************************/
201const 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/**********************************************************************/
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/**********************************************************************/
235
236
237/***************************************************************************/
238void FirstOrderLowPassFilter::init(const Vector &y0)
239{
240 if (filter!=NULL)
241 filter->init(y0);
242}
243
244
245/**********************************************************************/
246bool 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/**********************************************************************/
259bool 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/**********************************************************************/
272const 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/***************************************************************************/
296MedianFilter::MedianFilter(const size_t n, const Vector &y0)
297{
298 this->n=n;
299 init(y0);
300}
301
302
303/***************************************************************************/
304void 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/***************************************************************************/
315void MedianFilter::setOrder(const size_t n)
316{
317 this->n=n;
318 init(y);
319}
320
321
322/***************************************************************************/
323double 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/***************************************************************************/
338const 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
#define M_PI
Definition XSensMTx.cpp:24
IIR and FIR.
Definition filters.h:77
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
Definition filters.cpp:140
yarp::sig::Vector a
Definition filters.h:80
void setCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den)
Sets new filter coefficients.
Definition filters.cpp:99
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
void getCoeffs(yarp::sig::Vector &num, yarp::sig::Vector &den)
Returns the current filter coefficients.
Definition filters.cpp:91
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
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
yarp::sig::Vector b
Definition filters.h:79
void getStates(std::deque< yarp::sig::Vector > &u, std::deque< yarp::sig::Vector > &y)
Returns the current filter states.
Definition filters.cpp:132
yarp::sig::Vector y
Definition filters.h:81
std::deque< yarp::sig::Vector > yold
Definition filters.h:84
std::deque< yarp::sig::Vector > uold
Definition filters.h:83
bool setCutFrequency(const double cutFrequency)
Change the cut frequency of the filter.
Definition filters.cpp:246
bool setSampleTime(const double sampleTime)
Change the sample time of the filter.
Definition filters.cpp:259
FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime, const yarp::sig::Vector &y0=yarp::sig::Vector(1, 0.0))
Creates a filter with specified parameters.
Definition filters.cpp:218
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
Definition filters.cpp:238
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
Definition filters.cpp:272
virtual ~FirstOrderLowPassFilter()
Destructor.
Definition filters.cpp:231
yarp::sig::Vector y
Definition filters.h:322
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
Definition filters.cpp:304
std::deque< std::deque< double > > uold
Definition filters.h:321
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
Definition filters.cpp:338
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
void setOrder(const size_t n)
Sets new filter order.
Definition filters.cpp:315
double median(std::deque< double > &v)
Definition filters.cpp:323
yarp::sig::Vector uD
Definition filters.h:183
void setLimits(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU)
Sets new Rate limits.
Definition filters.cpp:193
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
yarp::sig::Vector rateLowerLim
Definition filters.h:186
virtual void init(const yarp::sig::Vector &u0)
Init internal state.
Definition filters.cpp:178
void getLimits(yarp::sig::Vector &rL, yarp::sig::Vector &rU)
Returns the current Rate limits.
Definition filters.cpp:185
yarp::sig::Vector uLim
Definition filters.h:184
yarp::sig::Vector rateUpperLim
Definition filters.h:185
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Limits the input rate.
Definition filters.cpp:201
zeros(2, 2) eye(2
int n