icub-basic-demos
IIRGausDeriv.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 // Copyright: (C) 2006-2007 Alex Bernardino, ISR-IST
4 // Authors: Alex Bernardino
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
18 #include <complex>
19 using namespace std;
20 
21 #include <iCub/IIRFilt.h>
22 
23 //
24 //Values of the pole locations for the iir recursive gaussian derivative filters of scale s = 2.
25 //From "Recursive Gaussian Derivative Filters", L.van.Vliet,Ian.T.Young and Piet.W.Verbeek
26 //
27 
28 // d - differentiation order
29 // N - number of poles
30 // L - approximation norm
31 
32 static const complex<double> d0_N3_L2[] = {
33  complex<double>(1.41650,1.00829),
34  complex<double>(1.41650,-1.00829),
35  complex<double>(1.86543,0),
36 };
37 
38 static const complex<double> d0_N4_L2[] = {
39  complex<double>(1.13228,1.28114),
40  complex<double>(1.13228,-1.28114),
41  complex<double>(1.78534,0.46763),
42  complex<double>(1.78534,-0.46763),
43 };
44 
45 static const complex<double> d0_N5_L2[] = {
46  complex<double>(0.86430,1.45389),
47  complex<double>(0.86430,-1.45389),
48  complex<double>(1.61433,0.83134),
49  complex<double>(1.61433,-0.83134),
50  complex<double>(1.87504,0),
51 };
52 
53 extern const complex<double> d0_N3_Linf[] = {
54  complex<double>(1.40098,1.00236),
55  complex<double>(1.40098,-1.00236),
56  complex<double>(1.85132,0),
57 };
58 
59 static const complex<double> d0_N4_Linf[] = {
60  complex<double>(1.12075,1.27788),
61  complex<double>(1.12075,-1.27788),
62  complex<double>(1.76952,0.46611),
63  complex<double>(1.76952,-0.46611),
64 };
65 
66 static const complex<double> d0_N5_Linf[] = {
67  complex<double>(0.85480,1.43749),
68  complex<double>(0.85480,-1.43749),
69  complex<double>(1.61231,0.82053),
70  complex<double>(1.61231,-0.82053),
71  complex<double>(1.87415,0)
72 };
73 
74 static const complex<double> d1_N3_Linf[] = {
75  complex<double>(1.31553,0.97057),
76  complex<double>(1.31553,-0.97057),
77  complex<double>(1.77635,0),
78 };
79 
80 static const complex<double> d1_N4_Linf[] = {
81  complex<double>(1.04185,1.24034),
82  complex<double>(1.04185,-1.24034),
83  complex<double>(1.69747,0.44790),
84  complex<double>(1.69747,-0.44790),
85 
86 };
87 
88 static const complex<double> d1_N5_Linf[] = {
89  complex<double>(0.77934,1.41423),
90  complex<double>(0.77934,-1.41423),
91  complex<double>(1.50941,0.80828),
92  complex<double>(1.50941,-0.80828),
93  complex<double>(1.77181,0)
94 };
95 
96 
97 static const complex<double> d3_N3_Linf[] = {
98  complex<double>(1.22886,0.93058),
99  complex<double>(1.22886,-0.93058),
100  complex<double>(1.70493,0),
101 };
102 
103 static const complex<double> d2_N4_Linf[] = {
104  complex<double>(0.94570,1.21064),
105  complex<double>(0.94570,-1.21064),
106  complex<double>(1.60161,0.42647),
107  complex<double>(1.60161,-0.42647),
108 };
109 
110 static const complex<double> d2_N5_Linf[] = {
111  complex<double>(0.69843,1.37655),
112  complex<double>(0.69843,-1.37655),
113  complex<double>(1.42631,0.77399),
114  complex<double>(1.42631,-0.77399),
115  complex<double>(1.69668,0)
116 };
117 
118 
119 
120 //compute the poles of the filter for scale s given the poles at scale 2
121 void calc_poles(int taps, const double scale, const complex<double> oldpoles[], complex<double> newpoles[])
122 {
123  if((taps < 3)||(taps>5))
124  throw "Invalid number of taps in calc_poles";
125 
126  if(newpoles == NULL)
127  throw "NULL Pointer argument in calc_poles";
128 
129  complex <double> d1_2, d2_2, d3_2, d4_2, d5_2;
130  d1_2 = oldpoles[0];
131  d2_2 = oldpoles[1];
132  d3_2 = oldpoles[2];
133  if(taps > 3)
134  d4_2 = oldpoles[3];
135  else
136  d4_2 = 0;
137  if(taps > 4)
138  d5_2 = oldpoles[4];
139  else
140  d5_2 = 0;
141 
142  double q, std, lambda;
143  double tol = 0.01;
144  complex <double> j(0,1), var;
145  complex <double> d1_s, d2_s, d3_s, d4_s, d5_s;
146  // computing new values for the poles
147  q = scale/2;
148  d1_s = exp(log(abs(d1_2))/q)*exp(j*arg(d1_2)/q);
149  d2_s = exp(log(abs(d2_2))/q)*exp(j*arg(d2_2)/q);
150  d3_s = exp(log(abs(d3_2))/q)*exp(j*arg(d3_2)/q);
151  if( abs(d4_2) != 0 )
152  d4_s = exp(log(abs(d4_2))/q)*exp(j*arg(d4_2)/q);
153  else
154  d4_s = 0;
155  if( abs(d5_2) != 0 )
156  d5_s = exp(log(abs(d5_2))/q)*exp(j*arg(d5_2)/q);
157  else
158  d5_s = 0;
159  // computing the variance of the new filter
160  var = d1_s*2.0/(d1_s-1.0)/(d1_s-1.0) + d2_s*2.0/(d2_s-1.0)/(d2_s-1.0) + d3_s*2.0/(d3_s-1.0)/(d3_s-1.0)+d4_s*2.0/(d4_s-1.0)/(d4_s-1.0)+d5_s*2.0/(d5_s-1.0)/(d5_s-1.0);
161  std = sqrt(var.real());
162  while( fabs(scale-std) > tol )
163  {
164  lambda = scale/std;
165  q = q*lambda;
166  // computing new values for the poles
167  d1_s = exp(log(abs(d1_2))/q)*exp(j*arg(d1_2)/q);
168  d2_s = exp(log(abs(d2_2))/q)*exp(j*arg(d2_2)/q);
169  d3_s = exp(log(abs(d3_2))/q)*exp(j*arg(d3_2)/q);
170  if( abs(d4_2) != 0)
171  d4_s = exp(log(abs(d4_2))/q)*exp(j*arg(d4_2)/q);
172  else
173  d4_s = 0;
174  if( abs(d5_2) != 0)
175  d5_s = exp(log(abs(d5_2))/q)*exp(j*arg(d5_2)/q);
176  else
177  d5_s = 0;
178  // computing the variance of the new filter
179  var = d1_s*2.0/(d1_s-1.0)/(d1_s-1.0) + d2_s*2.0/(d2_s-1.0)/(d2_s-1.0) + d3_s*2.0/(d3_s-1.0)/(d3_s-1.0)+d4_s*2.0/(d4_s-1.0)/(d4_s-1.0)+d5_s*2.0/(d5_s-1.0)/(d5_s-1.0);
180  std = sqrt(var.real());
181  }
182  newpoles[0] = d1_s;
183  newpoles[1] = d2_s;
184  newpoles[2] = d3_s;
185  newpoles[3] = d4_s;
186  newpoles[4] = d5_s;
187 }
188 
189 //compute the coefficients of the filter, given its poles
190 //the output is written in array coeffs - the first element is the gain and
191 //the remaining elements correspond to the autoregressive coefficients
192 void calc_coeffs(int taps, const complex<double> poles[], float *coeffs)
193 {
194  if((taps < 3)||(taps>5))
195  throw "Invalid number of taps in calc_coeffs";
196 
197  if(coeffs == NULL)
198  throw "NULL Pointer argument in calc_coeffs";
199 
200  complex <double> d1_s, d2_s, d3_s, d4_s, d5_s;
201  d1_s = poles[0];
202  d2_s = poles[1];
203  d3_s = poles[2];
204  if(taps > 3)
205  d4_s = poles[3];
206  else
207  d4_s = 0;
208  if(taps > 4)
209  d5_s = poles[4];
210  else
211  d5_s = 0;
212 
213  //computing the filter coeffs
214  if( taps == 3 )
215  {
216  complex<double> b = complex<double>(1.0,0.0)/d1_s/d2_s/d3_s;
217  coeffs[1] = (float)real(-b*(d2_s*d1_s + d3_s*d1_s + d3_s*d2_s));
218  coeffs[2] = (float)real(b*(d1_s + d2_s + d3_s));
219  coeffs[3] = (float)real(-b);
220  coeffs[4] = 0.0f;
221  coeffs[5] = 0.0f;
222  coeffs[0] = 1.0f + coeffs[1] + coeffs[2] + coeffs[3];
223  }
224  else if(taps == 4)
225  {
226  complex<double> b = complex<double>(1.0,0.0)/d1_s/d2_s/d3_s/d4_s;
227  coeffs[1] = (float)real(-b*(d3_s*d2_s*d1_s + d4_s*d2_s*d1_s + d4_s*d3_s*d1_s + d4_s*d3_s*d2_s));
228  coeffs[2] = (float)real(b*(d2_s*d1_s + d3_s*d1_s + d3_s*d2_s + d4_s*d1_s + d4_s*d2_s + d4_s*d3_s));
229  coeffs[3] = (float)real(-b*(d1_s + d2_s + d3_s + d4_s));
230  coeffs[4] = (float)real(b);
231  coeffs[5] = 0.0f;
232  coeffs[0] = 1.0f + coeffs[1] + coeffs[2] + coeffs[3] + coeffs[4];
233  }
234  else if(taps == 5)
235  {
236  complex <double> b = complex<double>(1.0,0.0)/d1_s/d2_s/d3_s/d4_s/d5_s;
237  coeffs[1] = (float)real(-b*(d4_s*d3_s*d2_s*d1_s + d5_s*d3_s*d2_s*d1_s + d5_s*d4_s*d2_s*d1_s + d5_s*d4_s*d3_s*d1_s + d5_s*d4_s*d3_s*d2_s));
238  coeffs[2] = (float)real(b*(d3_s*d2_s*d1_s + d4_s*d2_s*d1_s + d4_s*d3_s*d1_s + d4_s*d3_s*d2_s + d5_s*d2_s*d1_s + d5_s*d3_s*d1_s + d5_s*d3_s*d2_s + d5_s*d4_s*d1_s + d5_s*d4_s*d2_s + d5_s*d4_s*d3_s));
239  coeffs[3] = (float)real(-b*(d2_s*d1_s + d3_s*d1_s + d3_s*d2_s + d4_s*d1_s + d4_s*d2_s + d4_s*d3_s + d5_s*d1_s + d5_s*d2_s + d5_s*d3_s + d5_s*d4_s));
240  coeffs[4] = (float)real(b*(d1_s + d2_s + d3_s + d4_s + d5_s));
241  coeffs[5] = (float)real(-b);
242  coeffs[0] = 1.0f + coeffs[1] + coeffs[2] + coeffs[3] + coeffs[4] + coeffs[5];
243  }
244 }
245 
246 
247 
248 //compute the coefficients of the filter for scale s given the poles at scale 2
249 //the output is written in array coeffs - the first element is the gain and
250 //the remaining elements correspond to the autoregressive coefficients
251 void calc_coeffs(int taps, const complex<double> poles[], const double s, float *coeffs)
252 {
253 
254  if((taps < 3)||(taps>5))
255  throw "Invalid number of taps in calc_coeffs";
256 
257  if(coeffs == NULL)
258  throw "NULL Pointer argument in calc_coeffs";
259 
260  complex <double> d1_2, d2_2, d3_2, d4_2, d5_2;
261  d1_2 = poles[0];
262  d2_2 = poles[1];
263  d3_2 = poles[2];
264  if(taps > 3)
265  d4_2 = poles[3];
266  else
267  d4_2 = 0;
268  if(taps > 4)
269  d5_2 = poles[4];
270  else
271  d5_2 = 0;
272 
273  double q, std, lambda;
274  double tol = 0.01;
275  complex <double> j(0,1), var;
276  complex <double> d1_s, d2_s, d3_s, d4_s, d5_s;
277  // computing new values for the poles
278  q = s/2;
279  d1_s = exp(log(abs(d1_2))/q)*exp(j*arg(d1_2)/q);
280  d2_s = exp(log(abs(d2_2))/q)*exp(j*arg(d2_2)/q);
281  d3_s = exp(log(abs(d3_2))/q)*exp(j*arg(d3_2)/q);
282  if( abs(d4_2) != 0 )
283  d4_s = exp(log(abs(d4_2))/q)*exp(j*arg(d4_2)/q);
284  else
285  d4_s = 0;
286  if( abs(d5_2) != 0 )
287  d5_s = exp(log(abs(d5_2))/q)*exp(j*arg(d5_2)/q);
288  else
289  d5_s = 0;
290  // computing the variance of the new filter
291  var = d1_s*2.0/(d1_s-1.0)/(d1_s-1.0) + d2_s*2.0/(d2_s-1.0)/(d2_s-1.0) + d3_s*2.0/(d3_s-1.0)/(d3_s-1.0)+d4_s*2.0/(d4_s-1.0)/(d4_s-1.0)+d5_s*2.0/(d5_s-1.0)/(d5_s-1.0);
292  std = sqrt(var.real());
293  while( fabs(s-std) > tol )
294  {
295  lambda = s/std;
296  q = q*lambda;
297  // computing new values for the poles
298  d1_s = exp(log(abs(d1_2))/q)*exp(j*arg(d1_2)/q);
299  d2_s = exp(log(abs(d2_2))/q)*exp(j*arg(d2_2)/q);
300  d3_s = exp(log(abs(d3_2))/q)*exp(j*arg(d3_2)/q);
301  if( abs(d4_2) != 0)
302  d4_s = exp(log(abs(d4_2))/q)*exp(j*arg(d4_2)/q);
303  else
304  d4_s = 0;
305  if( abs(d5_2) != 0)
306  d5_s = exp(log(abs(d5_2))/q)*exp(j*arg(d5_2)/q);
307  else
308  d5_s = 0;
309  // computing the variance of the new filter
310  var = d1_s*2.0/(d1_s-1.0)/(d1_s-1.0) + d2_s*2.0/(d2_s-1.0)/(d2_s-1.0) + d3_s*2.0/(d3_s-1.0)/(d3_s-1.0)+d4_s*2.0/(d4_s-1.0)/(d4_s-1.0)+d5_s*2.0/(d5_s-1.0)/(d5_s-1.0);
311  std = sqrt(var.real());
312  }
313 
314  //computing the filter coeffs
315  if( taps == 3 )
316  {
317  complex<double> b = complex<double>(1.0,0.0)/d1_s/d2_s/d3_s;
318  coeffs[1] = (float)real(-b*(d2_s*d1_s + d3_s*d1_s + d3_s*d2_s));
319  coeffs[2] = (float)real(b*(d1_s + d2_s + d3_s));
320  coeffs[3] = (float)real(-b);
321  coeffs[4] = 0.0f;
322  coeffs[5] = 0.0f;
323  coeffs[0] = 1.0f + coeffs[1] + coeffs[2] + coeffs[3];
324  }
325  else if(taps == 4)
326  {
327  complex<double> b = complex<double>(1.0,0.0)/d1_s/d2_s/d3_s/d4_s;
328  coeffs[1] = (float)real(-b*(d3_s*d2_s*d1_s + d4_s*d2_s*d1_s + d4_s*d3_s*d1_s + d4_s*d3_s*d2_s));
329  coeffs[2] = (float)real(b*(d2_s*d1_s + d3_s*d1_s + d3_s*d2_s + d4_s*d1_s + d4_s*d2_s + d4_s*d3_s));
330  coeffs[3] = (float)real(-b*(d1_s + d2_s + d3_s + d4_s));
331  coeffs[4] = (float)real(b);
332  coeffs[5] = 0.0f;
333  coeffs[0] = 1.0f + coeffs[1] + coeffs[2] + coeffs[3] + coeffs[4];
334  }
335  else if(taps == 5)
336  {
337  complex <double> b = complex<double>(1.0,0.0)/d1_s/d2_s/d3_s/d4_s/d5_s;
338  coeffs[1] = (float)real(-b*(d4_s*d3_s*d2_s*d1_s + d5_s*d3_s*d2_s*d1_s + d5_s*d4_s*d2_s*d1_s + d5_s*d4_s*d3_s*d1_s + d5_s*d4_s*d3_s*d2_s));
339  coeffs[2] = (float)real(b*(d3_s*d2_s*d1_s + d4_s*d2_s*d1_s + d4_s*d3_s*d1_s + d4_s*d3_s*d2_s + d5_s*d2_s*d1_s + d5_s*d3_s*d1_s + d5_s*d3_s*d2_s + d5_s*d4_s*d1_s + d5_s*d4_s*d2_s + d5_s*d4_s*d3_s));
340  coeffs[3] = (float)real(-b*(d2_s*d1_s + d3_s*d1_s + d3_s*d2_s + d4_s*d1_s + d4_s*d2_s + d4_s*d3_s + d5_s*d1_s + d5_s*d2_s + d5_s*d3_s + d5_s*d4_s));
341  coeffs[4] = (float)real(b*(d1_s + d2_s + d3_s + d4_s + d5_s));
342  coeffs[5] = (float)real(-b);
343  coeffs[0] = 1.0f + coeffs[1] + coeffs[2] + coeffs[3] + coeffs[4] + coeffs[5];
344  }
345 }
346 
347 
348 
349 
350 
IIRFilt.h
Functions for generic, 3 tap, iir filtering.
calc_poles
void calc_poles(int taps, const double scale, const complex< double > oldpoles[], complex< double > newpoles[])
5 tap second derivative filter with Linf norm approximation
Definition: IIRGausDeriv.cpp:121
calc_coeffs
void calc_coeffs(int taps, const complex< double > poles[], float *coeffs)
Compute the coefficients of the filter, given its poles.
Definition: IIRGausDeriv.cpp:192
d0_N4_L2
const complex< double > d0_N4_L2[]
3 tap gaussian filter with L2 norm approximation
d1_N4_Linf
const complex< double > d1_N4_Linf[]
3 tap first derivative filter with Linf norm approximation
d0_N5_Linf
const complex< double > d0_N5_Linf[]
4 tap gaussian filter with Linf norm approximation
d0_N4_Linf
const complex< double > d0_N4_Linf[]
3 tap gaussian filter with Linf norm approximation
d0_N5_L2
const complex< double > d0_N5_L2[]
4 tap gaussian filter with L2 norm approximation
d0_N3_Linf
const complex< double > d0_N3_Linf[]
5 tap gaussian filter with L2 norm approximation
d1_N5_Linf
const complex< double > d1_N5_Linf[]
4 tap first derivative filter with Linf norm approximation
d2_N5_Linf
const complex< double > d2_N5_Linf[]
4 tap second derivative filter with Linf norm approximation
d2_N4_Linf
const complex< double > d2_N4_Linf[]
3 tap second derivative filter with Linf norm approximation
d1_N3_Linf
const complex< double > d1_N3_Linf[]
5 tap gaussian filter with Linf norm approximation
d0_N3_L2
const complex< double > d0_N3_L2[]
Coefficients for scale = 2 filters.
Generated by doxygen 1.9.1