IT++ Logo
filter_design.cpp
Go to the documentation of this file.
1 
30 #include <itpp/signal/poly.h>
31 #include <itpp/signal/filter.h>
32 #include <itpp/signal/transforms.h>
35 #include <itpp/base/matfunc.h>
36 #include <itpp/base/specmat.h>
38 #include <itpp/base/converters.h>
39 
40 
41 namespace itpp
42 {
43 
44 
45 void polystab(const vec &a, vec &out)
46 {
47  cvec r;
48  roots(a, r);
49 
50  for (int i = 0; i < r.size(); i++) {
51  if (abs(r(i)) > 1)
52  r(i) = std::complex<double>(1.0) / conj(r(i));
53  }
54  out = real(std::complex<double>(a(0)) * poly(r));
55 }
56 
57 void polystab(const cvec &a, cvec &out)
58 {
59  cvec r;
60  roots(a, r);
61 
62  for (int i = 0; i < r.size(); i++) {
63  if (abs(r(i)) > 1)
64  r(i) = std::complex<double>(1.0) / conj(r(i));
65  }
66  out = a(0) * poly(r);
67 }
68 
69 
70 // ----------------------- freqz() ---------------------------------------------------------
71 void freqz(const cvec &b, const cvec &a, const int N, cvec &h, vec &w)
72 {
73  w = pi * linspace(0, N - 1, N) / double(N);
74 
75  cvec ha, hb;
76  hb = fft(b, 2 * N);
77  hb = hb(0, N - 1);
78 
79  ha = fft(a, 2 * N);
80  ha = ha(0, N - 1);
81 
82  h = elem_div(hb, ha);
83 }
84 
85 cvec freqz(const cvec &b, const cvec &a, const int N)
86 {
87  cvec h;
88  vec w;
89 
90  freqz(b, a, N, h, w);
91 
92  return h;
93 }
94 
95 
96 cvec freqz(const cvec &b, const cvec &a, const vec &w)
97 {
98  int la = a.size(), lb = b.size(), k = std::max(la, lb);
99 
100  cvec h, ha, hb;
101 
102  // Evaluate the nominator and denominator at the given frequencies
103  hb = polyval(zero_pad(b, k), to_cvec(cos(w), sin(w)));
104  ha = polyval(zero_pad(a, k), to_cvec(cos(w), sin(w)));
105 
106  h = elem_div(hb, ha);
107 
108  return h;
109 }
110 
111 void freqz(const vec &b, const vec &a, const int N, cvec &h, vec &w)
112 {
113  w = pi * linspace(0, N - 1, N) / double(N);
114 
115  cvec ha, hb;
116  hb = fft_real(b, 2 * N);
117  hb = hb(0, N - 1);
118 
119  ha = fft_real(a, 2 * N);
120  ha = ha(0, N - 1);
121 
122  h = elem_div(hb, ha);
123 }
124 
125 cvec freqz(const vec &b, const vec &a, const int N)
126 {
127  cvec h;
128  vec w;
129 
130  freqz(b, a, N, h, w);
131 
132  return h;
133 }
134 
135 
136 cvec freqz(const vec &b, const vec &a, const vec &w)
137 {
138  int la = a.size(), lb = b.size(), k = std::max(la, lb);
139 
140  cvec h, ha, hb;
141 
142  // Evaluate the nominator and denominator at the given frequencies
143  hb = polyval(zero_pad(b, k), to_cvec(cos(w), sin(w)));
144  ha = polyval(zero_pad(a, k), to_cvec(cos(w), sin(w)));
145 
146  h = elem_div(hb, ha);
147 
148  return h;
149 }
150 
151 
152 
153 void filter_design_autocorrelation(const int N, const vec &f, const vec &m, vec &R)
154 {
155  it_assert(f.size() == m.size(), "filter_design_autocorrelation: size of f and m vectors does not agree");
156  int N_f = f.size();
157 
158  it_assert(f(0) == 0.0, "filter_design_autocorrelation: first frequency must be 0.0");
159  it_assert(f(N_f - 1) == 1.0, "filter_design_autocorrelation: last frequency must be 1.0");
160 
161  // interpolate frequency-response
162  int N_fft = 512;
163  vec m_interp(N_fft + 1);
164  // unused variable:
165  // double df_interp = 1.0/double(N_fft);
166 
167  m_interp(0) = m(0);
168  double inc;
169 
170  int jstart = 0, jstop;
171 
172  for (int i = 0; i < N_f - 1; i++) {
173  // calculate number of points to the next frequency
174  jstop = floor_i(f(i + 1) * (N_fft + 1)) - 1;
175  //std::cout << "jstart=" << jstart << "jstop=" << jstop << std::endl;
176 
177  for (int j = jstart; j <= jstop; j++) {
178  inc = double(j - jstart) / double(jstop - jstart);
179  m_interp(j) = m(i) * (1 - inc) + m(i + 1) * inc;
180  }
181  jstart = jstop + 1;
182  }
183 
184  vec S = sqr(concat(m_interp, reverse(m_interp(2, N_fft)))); // create a complete frequency response with also negative frequencies
185 
186  R = ifft_real(to_cvec(S)); // calculate correlation
187 
188  R = R.left(N);
189 }
190 
191 
192 // Calculate the AR coefficients of order \c n of the ARMA-process defined by the autocorrelation R
193 // using the deternined modified Yule-Walker method
194 // maxlag determines the size of the system to solve N>= n.
195 // If N>m then the system is overdetermined and a least squares solution is used.
196 // as a rule of thumb use N = 4*n
197 void modified_yule_walker(const int m, const int n, const int N, const vec &R, vec &a)
198 {
199  it_assert(m > 0, "modified_yule_walker: m must be > 0");
200  it_assert(n > 0, "modified_yule_walker: n must be > 0");
201  it_assert(N <= R.size(), "modified_yule_walker: autocorrelation function too short");
202 
203  // create the modified Yule-Walker equations Rm * a = - rh
204  // see eq. (3.7.1) in Stoica and Moses, Introduction to spectral analysis
205  int M = N - m - 1;
206 
207  mat Rm;
208  if (m - n + 1 < 0)
209  Rm = toeplitz(R(m, m + M - 1), reverse(concat(R(1, std::abs(m - n + 1)), R(0, m))));
210  else
211  Rm = toeplitz(R(m, m + M - 1), reverse(R(m - n + 1, m)));
212 
213 
214  vec rh = - R(m + 1, m + M);
215 
216  // solve overdetermined system
217  a = backslash(Rm, rh);
218 
219  // prepend a_0 = 1
220  a = concat(1.0, a);
221 
222  // stabilize polynomial
223  a = polystab(a);
224 }
225 
226 
227 
228 void arma_estimator(const int m, const int n, const vec &R, vec &b, vec &a)
229 {
230  it_assert(m > 0, "arma_estimator: m must be > 0");
231  it_assert(n > 0, "arma_estimator: n must be > 0");
232  it_assert(2*(m + n) <= R.size(), "arma_estimator: autocorrelation function too short");
233 
234 
235  // windowing the autocorrelation
236  int N = 2 * (m + n);
237  vec Rwindow = elem_mult(R.left(N), 0.54 + 0.46 * cos(pi * linspace(0.0, double(N - 1), N) / double(N - 1))); // Hamming windowing
238 
239  // calculate the AR part using the overdetmined Yule-Walker equations
240  modified_yule_walker(m, n, N, Rwindow, a);
241 
242  // --------------- Calculate MA part --------------------------------------
243  // use method in ref [2] section VII.
244  vec r_causal = Rwindow;
245  r_causal(0) *= 0.5;
246 
247  vec h_inv_a = filter(1, a, concat(1.0, zeros(N - 1))); // see eq (50) of [2]
248  mat H_inv_a = toeplitz(h_inv_a, concat(1.0, zeros(m)));
249 
250  vec b_causal = backslash(H_inv_a, r_causal);
251 
252  // calculate the double-sided spectrum
253  int N_fft = 256;
254  vec H = 2.0 * real(elem_div(fft_real(b_causal, N_fft), fft_real(a, N_fft))); // calculate spectrum
255 
256  // Do weighting and windowing in cepstrum domain
257  cvec cepstrum = log(to_cvec(H));
258  cvec q = ifft(cepstrum);
259 
260  // keep only causal part of spectrum (windowing)
261  q.set_subvector(N_fft / 2, zeros_c(N_fft / 2));
262  q(0) *= 0.5;
263 
264  cvec h = ifft(exp(fft(q))); // convert back to frequency domain, from cepstrum and do inverse transform to calculate impulse response
265  b = real(backslash(to_cmat(H_inv_a), h(0, N - 1))); // use Shank's method to calculate b coefficients
266 }
267 
268 
269 void yulewalk(const int N, const vec &f, const vec &m, vec &b, vec &a)
270 {
271  it_assert(f.size() == m.size(), "yulewalk: size of f and m vectors does not agree");
272  int N_f = f.size();
273 
274  it_assert(f(0) == 0.0, "yulewalk: first frequency must be 0.0");
275  it_assert(f(N_f - 1) == 1.0, "yulewalk: last frequency must be 1.0");
276 
277 
278  vec R;
279  filter_design_autocorrelation(4*N, f, m, R);
280 
281  arma_estimator(N, N, R, b, a);
282 }
283 
284 
285 } // namespace itpp
SourceForge Logo

Generated on Sat Jul 6 2013 10:54:25 for IT++ by Doxygen 1.8.2