CEBL  2.1
PassBandFunctions.cpp
Go to the documentation of this file.
1 #include "PassBandFunctions.hpp"
2 
3 #include <iostream>
4 using namespace std;
5 using namespace cppR;
6 
7 
8 /**************************************************/
9 
10 double const pi = M_PI;
11 
12 template <typename T>
13 double logb(int base, T value)
14 {
15  return log(value)/log(base);
16 }
17 template <typename T>
18 double log2(T value)
19 {
20  return logb(2,value);
21 }
22 
23 
24 
25 /**************************************************/
26 
27 // ORIGINAL DOCUMENTATION FROM chebbp2.m
28 // Source: http://www.dsp.rice.edu/software/fir.shtml
29 
30 // A second program for the design of symmetric bandpass FIR filters with
31 // flat monotonically decreasing passbands (on either side of wp)
32 // and equiripple stopbands. This program is similar to chebbp.m,
33 // but it uses a different set of input parameters.
34 //
35 // output
36 // h : filter
37 // input
38 // N : length of total filter
39 // L : degree of flatness
40 // wp : passband frequency of flatness
41 // ws1 : first stopband edge
42 // ws2 : second stopband edge
43 // Need: 0 < ws1 < wp < ws2 < pi
44 //
45 // Author: Ivan W. Selesnick, Rice University
46 
47 
48 
49 Matrix chebbp2(int N, int L, double wp, double ws1, double ws2)
50 {
51 
52  //check numbers are in the right range
53  if((N%2)==0 || (L%4) != 0)
54  {
55  cerr << "N must be odd and L must be divisible by 4\n";
56  return Matrix(0,0);
57  }
58  if(0 >= ws1 || ws1 >= wp || wp >= ws2 || ws2 >= pi)
59  {
60  cerr << "need: 0 < ws1 < wp < ws2 < pi\n";
61  return Matrix(0,0);
62  }
63  if(L < 1)
64  {
65  cerr << "L must be positive\n";
66  return Matrix(0,0);
67  }
68 
69 
70  int q = int((N-L + 1) / 2);
71  int g = int(pow(2,ceil(log2(8*N)))); //number of grid points
72  double SN = 1e-8; //SMALL NUMBER
73 
74  //w <- t(0:g) * pi / g
75  Matrix w(1, g+1);
76  for(int i=0; i<w.size2(); i++)
77  w(0,i) = i * pi / g;
78 
79  double d = ws1 / (pi-ws2);
80  int q1 = int(round((q+1)/(1+1/d)));
81 
82  if (q1 == 0)
83  q1 = 1;
84  else if (q1 == q+1)
85  q1 = q;
86 
87  int q2 = q + 1 - q1;
88 
89 
90  Matrix rs1(1,q1);
91  if(q1 == 1)
92  {
93  rs1.resize(1,1);
94  rs1(0,0) = ws1;
95  }
96  else
97  {
98  for(int i=0; i<rs1.size2(); i++)
99  rs1(0,i) = i * (ws1/(q1-1));
100  }
101  Matrix rs2(1,q2);
102  if(q2 == 1)
103  {
104  rs2.resize(1,1);
105  rs2(0,0) = ws2;
106  }
107  else
108  {
109  for(int i=0; i<rs2.size2(); i++)
110  rs2(0,i) = i * ((pi-ws2)/(q2-1)) + ws2;
111  }
112 
113 
114  Matrix rs(rs1.size2() + rs2.size2(),1);
115  for(int i=0;i<rs1.size2();i++)
116  rs(i,0) = rs1(0,i);
117  for(int i=0;i<rs2.size2();i++)
118  rs(i+rs1.size2(),0) = rs2(0,i);
119 
120 
121  //******************************
122 
123  //Matrix Z(int(2 * (g+1-q))-1,1);
124  int Z_size = 2 * (g+1-q)-1;
125 
126  // A1 <- (-1)^(L/2) * (sin(w/2-wp/2) * sin(w/2+wp/2)) ^(L/2)
127  Matrix temp = compProd(apply(Matrix(w/2) - double(wp/2),sin),
128  apply(Matrix(w/2) + double(wp/2),sin));
129  Matrix A1 = apply(temp, pow, L/2) * pow(-1.0,L/2);
130 
131 
132  //si = q iterations of alternating +1, -1
133  Matrix si(q+1,1);
134  for(int i=0;i<si.size1();i++)
135  si(i,0) = (i % 2)==0 ? 1 : -1;
136 
137  //n = single row of numbers 0:q-1
138  Matrix n(1,q);
139  for(int i=0;i<n.size2();i++)
140  n(0,i) = i;
141 
142  //A1r <- (-1)^(L/2) * (sin(rs/2-wp/2) * sin(rs/2+wp/2))^(L/2)
143  temp = compProd(apply(Matrix(rs/2) - double(wp/2),sin),
144  apply(Matrix(rs/2) + double(wp/2),sin));
145  Matrix A1r = apply(temp, pow, L/2) * pow(-1.0,L/2);
146 
147  //******************************
148  Matrix a;
149 
150  for(int it=0; it<15; it++)
151  {
152  temp = cbind(Matrix(apply(Matrix(prod(rs,n)),cos)),
153  Matrix(compDiv(si,A1r)));
154  Matrix x = solve(temp,compDiv(1.0,A1r));
155 
156  a.resize(x.size1()-1,1);
157  for(int i=0;i<a.size1();i++)
158  a(i,0) = -x(i,0);
159 
160  double del = x(q,0);
161  //matrix to compute fft of
162  Matrix F;
163  if(q > 2)
164  {
165  F = createMatrix(0,2*q-1 + Z_size,1);
166  F(0,0) = a(0,0);
167  //secondcomp
168  int i;
169  for(i=1;i<q;i++)
170  {
171  F(i,0) = a(i,0)/2;
172  F(F.size1()-i,0) = a(i,0)/2;
173  }
174  }
175  else
176  {
177  F = createMatrix(0,1 + Z_size,1);
178  F(0,0) = a(0,0);
179  }
180 
181  //compute fft of F and save the real part
182  Matrix A2 = Re(fft(F));
183  A2 = submatrix(A2,0,g,0,0);
184 
185  //1 X g matrix A
186  Matrix A = compProd(A1,t(A2)) + 1.0;
187  Matrix Y = si*del;
188 
189  //local max of A and A-1
190  std::vector<int> lmA = localMax(A);
191 
192  std::vector<int> lmAInv = localMax(A*(-1));
193  for(int i=0;i<lmAInv.size();i++)
194  lmA.push_back(lmAInv[i]);
195  sort(lmA.begin(),lmA.end());
196 
197 
198  int llmA = lmA.size();
199  //cout << "llmA = " << llmA << "\n";
200  //cout << "A SIZE: " << A.size1() <<"," << A.size2() << "\n";
201  if(llmA != q+1)
202  {
203  /*cout << "A SIZE: " << A.size1() <<"," << A.size2() << "\n";
204  int ind1 = lmA[llmA-1]-1;
205  int ind2 = lmA[llmA-2]-1;
206  int ind3 = lmA[0]-1;
207  int ind4 = lmA[1]-1;
208  cout << "INDS: " << ind1 << ", " << ind2 << ", " << ind3 << ", " << ind4 << "\n";
209  */
210  if(abs(A(0,lmA[llmA-1]-1)-A(0,lmA[llmA-2]-1))
211  < abs(A(0,lmA[0]-1)-A(0,lmA[1]-1)))
212  {
213  lmA.resize(llmA-1);
214  }
215  else
216  {
217  lmA.erase(lmA.begin());
218  }
219  }
220 
221  //ri is the indexes of A which are local max for A and -A
222  std::vector<double> ri(lmA.size());
223  int k = 0;
224  double min = DBL_MAX;
225  for(int i=0;i<lmA.size();i++)
226  {
227  ri[i] = (double(lmA[i])-1.0) * pi/g;
228  double v = abs(ri[i]-wp);
229  if(v < min)
230  {
231  min = v;
232  k = i;
233  }
234  }
235 
236  ri.erase(ri.begin()+k);
237 
238  //compute Aws1 and Aws2
239  int sign = int(pow(-1.0, L/2));
240  double Aws1 = (prod(apply(Matrix(n*ws1),cos), a))(0,0) * sign * pow(sin(ws1/2-wp/2) * sin(ws1/2 + wp/2), L/2) + 1;
241  double Aws2 = (prod(apply(Matrix(n*ws2),cos), a))(0,0) * sign * pow(sin(ws2/2-wp/2) * sin(ws2/2 + wp/2), L/2) + 1;
242 
243  //check in any values of ri are between wp and ws2
244  bool in_range = false;
245  for(int i=0;i<ri.size();i++)
246  if(ri[i] > wp && ri[i] < ws2)
247  {
248  in_range = true;
249  break;
250  }
251 
252  //add ws1 or ws2
253  if((Aws1 > Aws2) || (in_range))
254  ri.push_back(ws1);
255  else
256  ri.push_back(ws2);
257 
258  sort(ri.begin(),ri.end());
259 
260  //copy ri to rs
261  //cout << "ri.size() = " << ri.size() << ", rs.size1() = " << rs.size1() << "\n";
262  for(int i=0;i<ri.size();i++)
263  rs(i,0) = ri[i];
264 
265  A1r = apply(compProd(apply(Matrix(rs * (.5)) - (wp/2),sin),
266  apply(Matrix(rs * (.5)) + (wp/2),sin))
267  , pow
268  , L/2) * sign;
269 
270  Matrix Ar = Matrix(compProd(Matrix(prod(apply(Matrix(prod(rs,n)),cos),a)), A1r)) + 1.0;
271  double armax = cppR::max(Ar-abs(del));
272  double e1 = abs(del)-abs(cppR::min(Ar));
273 
274  double Err = armax > e1 ? armax : e1;
275  /*if(it == 3)
276  {
277  Matrix temp = Ar-abs(del);
278  for(int i=0;i<temp.size1();i++)
279  {
280  cout << (i+1) << "] " << temp(i,0) << "\n";
281  }
282  }*/
283  //cout << "abs(del) = " << abs(del) << "\n";
284  //cout << "armax = " << armax << " e1 = " << e1 << "\n";
285  cout << "\tErr is " << Err << "\n";
286  if(Err < SN)
287  {
288  cout << "\tI have converged\n";
289  break;
290  }
291  }//end for loop
292  /* for(int i=0;i<a.size1();i++)
293  {
294  cout << (i+1) << "] " << a(i,0) << "\n";
295  }*/
296 
297  //set up h value vector for convolution
298  Matrix h;
299  if(q > 2)
300  {
301  h.resize(q*2-1,1);
302  int h_index = 0;
303  for(int i=q-1;i>=1;i--)
304  {
305  h(h_index++,0) = a(i,0)/2;
306  }
307  h(h_index++,0) = a(0,0);
308  for(int i=1;i<q;i++)
309  {
310  h(h_index++,0) = a(i,0)/2;
311  }
312  }
313  else
314  {
315  h.resize(1,1);
316  h(0,0) = a(0,0);
317  }
318 
319  //set up y vector for convolution
320  Matrix y(3,1);
321  y(0,0) = 1;
322  y(1,0) = -2 * cos(wp);
323  y(2,0) = 1;
324 
325  //do the convolutions
326  for(int i=0;i<L/2;i++)
327  {
328  h = convolve(h, y) * (1.0/4.0);
329  }
330  int ind = ((N+1)/2) - 1;
331  h(ind,0) += 1.0;
332 
333  return(h);
334 
335 }
336 
337 /**************************************************/
338 
339 std::vector<int> localMax(ublas::matrix<double> X)
340 {
341  std::vector<int> k;
342 
343  if(X.size1() < X.size2())
344  X = t(X);
345  int size = X.size1();
346 
347  for(int i=0;i<size-2;i++)
348  {
349  bool b1 = X(i,0) <= X(i+1,0);
350  bool b2 = X(i+1,0) > X(i+2,0);
351  if(b1 && b2)
352  k.push_back(i+2);
353  }
354 
355  if(X(0,0) > X(1,0))
356  k.push_back(1);
357  if(X(size-1,0) > X(size-2,0))
358  k.push_back(size);
359 
360  std::sort(k.begin(), k.end());
361 
362  return k;
363 }
364 
365 
366 /**************************************************/
367 
368 Matrix makePassband(int N, int L, int Fs, double fstop1, double fpass, double fstop2)
369 {
370 
371  printf("fstop1=%f, fpass=%f, fstop2=%f\n",fstop1, fpass, fstop2);
372  double wp = fpass /(Fs/2) * pi;
373  double ws1 = fstop1/(Fs/2) * pi;
374  double ws2 = fstop2/(Fs/2) * pi;
375 
376  printf("wp: %f, ws1: %f, ws2 %f\n",wp,ws1,ws2);
377 
378  return(chebbp2(N,L,wp,ws1,ws2));
379 }
380 
381 
382 /*************************************************/
383 //convolve
384 ublas::matrix<double> convolve(ublas::matrix<double> x, ublas::matrix<double> y)
385 {
386  /*
387  ASSUME:
388  x,y are numeric
389  conj = true
390  type = open
391 
392  */
393 
394  int nx = x.size1();
395  int ny = y.size1();
396  int n = nx;
397 
398  //type is open
399  Matrix x1(ny + nx - 1,1);
400 
401  int x1_index = 0;
402  for(int i=0;i<ny-1;i++)
403  {
404  x1(x1_index++,0) = 0;
405  }
406  for(int i=0; i<nx; i++)
407  {
408  x1(x1_index++,0) = x(i,0);
409  }
410 
411  if(nx > 1)
412  {
413  y.resize(ny + nx - 1, 1);
414  for(int i=0;i<nx-1;i++)
415  y(ny + i,0) = 0;
416  n = y.size1();
417  }
418 
419  ublas::matrix< complex<double> > xfft1 = fft(x1);
420  //assume conj=TRUE
421  ublas::matrix< complex<double> > yfft1 = Conj(fft(y));
422 
423  //assume real=TRUE
424  ublas::matrix<double> ret = Re(fft(compProd(xfft1,yfft1),true)) * (1.0/n);
425 
426  return ret;
427 }
428 
429 /**************************************************/
430 
431 
433 {
434  Matrix A = createMatrix(1,1,1);
435 
436  if(B.size2() != 1 || A.size2() != 1)
437  throw("B.size2() == 1 or A.size2() == 1");
438 
439  int nB = B.size1();
440  int nA = A.size1();
441 
442  int nX = x.size2();
443  int nDim = x.size1();
444 
445  Matrix state_x;
446  Matrix state_y;
447  if(state.empty)
448  {
449  state_x = createMatrix(0.0, nDim, nB-1);
450  state_y = createMatrix(0.0, nDim, nA-1);
451  }
452  else
453  {
454  state_x = state.x;
455  state_y = state.y;
456  }
457  if(state_x.size1() == 0 || state_x.size2() == 0)
458  state_x = createMatrix(0.0, nDim, nB-1);
459  if(state_y.size1() == 0 || state_y.size2() == 0)
460  state_y = createMatrix(0.0, nDim, nA-1);
461 
462  state_x = cbind(state_x, x);
463  int ncsx = ncol(state_x);
464 
465  Matrix v = x * B(0,0);
466  //cout << "B(0,0) = " << B(0,0) << "\n";
467  //cout << "v(0,0) = " << v(0,0) << "\n";
468 
469  writeTable(v,"v.txt");
470  if(nB > 1)
471  {
472  for(int i=1; i<nB; i++)
473  {
474  v = v + submatrix(state_x, 0, 0, ncsx-i-nX+1, ncsx-i) * B(i,0);
475  }
476  state_x = submatrix(state_x,0,0, ncsx-nB+1, ncsx-1);
477  }
478  state_y = cbind(state_y, v);
479  if(nA > 1)
480  {
481  Matrix Arev = -rev(Matrix(submatrix(A,1,0,0,0)));
482  for(int i=0; i<nX; i++)
483  {
484  submatrix(state_y, 0, 0, nA+i-2, nA+i-2) = submatrix(state_y, 0, 0, nA+i-2, nA+i-2)
485  + prod(submatrix(state_y, 0, 0, i, i+nA-3) , Arev);
486  }
487  }
488  int ncsy = ncol(state_y);
489  Matrix y = submatrix(state_y,0,0, ncsy-nX, ncsy-1);
490  /*writeTable(x,"x.txt");
491  writeTable(state_y,"sty.txt");
492  writeTable(y,"y.txt");*/
493  if(nA > 1)
494  state_y = submatrix(state_y,0,0,ncsy-nA, ncsy-1);
495  else
496  state_y.resize(0,0);
497 
498 
499  //Return values
500  state.x = state_x;
501  state.y = state_y;
502  state.empty = false;
503  FilterResult res;
504  res.state = state;
505  res.filtered = y;
506  return(res);
507 }
508