IT++ Logo
filter_design.cpp
Go to the documentation of this file.
00001 
00029 #include <itpp/signal/filter_design.h>
00030 #include <itpp/signal/poly.h>
00031 #include <itpp/signal/filter.h>
00032 #include <itpp/signal/transforms.h>
00033 #include <itpp/base/math/elem_math.h>
00034 #include <itpp/base/algebra/ls_solve.h>
00035 #include <itpp/base/matfunc.h>
00036 #include <itpp/base/specmat.h>
00037 #include <itpp/base/math/trig_hyp.h>
00038 #include <itpp/base/converters.h>
00039 
00040 
00041 namespace itpp
00042 {
00043 
00044 
00045 void polystab(const vec &a, vec &out)
00046 {
00047   cvec r;
00048   roots(a, r);
00049 
00050   for (int i = 0; i < r.size(); i++) {
00051     if (abs(r(i)) > 1)
00052       r(i) = std::complex<double>(1.0) / conj(r(i));
00053   }
00054   out = real(std::complex<double>(a(0)) * poly(r));
00055 }
00056 
00057 void polystab(const cvec &a, cvec &out)
00058 {
00059   cvec r;
00060   roots(a, r);
00061 
00062   for (int i = 0; i < r.size(); i++) {
00063     if (abs(r(i)) > 1)
00064       r(i) = std::complex<double>(1.0) / conj(r(i));
00065   }
00066   out = a(0) * poly(r);
00067 }
00068 
00069 
00070 // ----------------------- freqz() ---------------------------------------------------------
00071 void freqz(const cvec &b, const cvec &a, const int N, cvec &h, vec &w)
00072 {
00073   w = pi * linspace(0, N - 1, N) / double(N);
00074 
00075   cvec ha, hb;
00076   hb = fft(b, 2 * N);
00077   hb = hb(0, N - 1);
00078 
00079   ha = fft(a, 2 * N);
00080   ha = ha(0, N - 1);
00081 
00082   h = elem_div(hb, ha);
00083 }
00084 
00085 cvec freqz(const cvec &b, const cvec &a, const int N)
00086 {
00087   cvec h;
00088   vec w;
00089 
00090   freqz(b, a, N, h, w);
00091 
00092   return h;
00093 }
00094 
00095 
00096 cvec freqz(const cvec &b, const cvec &a, const vec &w)
00097 {
00098   int la = a.size(), lb = b.size(), k = std::max(la, lb);
00099 
00100   cvec h, ha, hb;
00101 
00102   // Evaluate the nominator and denominator at the given frequencies
00103   hb = polyval(zero_pad(b, k), to_cvec(cos(w), sin(w)));
00104   ha = polyval(zero_pad(a, k), to_cvec(cos(w), sin(w)));
00105 
00106   h = elem_div(hb, ha);
00107 
00108   return h;
00109 }
00110 
00111 void freqz(const vec &b, const vec &a, const int N, cvec &h, vec &w)
00112 {
00113   w = pi * linspace(0, N - 1, N) / double(N);
00114 
00115   cvec ha, hb;
00116   hb = fft_real(b, 2 * N);
00117   hb = hb(0, N - 1);
00118 
00119   ha = fft_real(a, 2 * N);
00120   ha = ha(0, N - 1);
00121 
00122   h = elem_div(hb, ha);
00123 }
00124 
00125 cvec freqz(const vec &b, const vec &a, const int N)
00126 {
00127   cvec h;
00128   vec w;
00129 
00130   freqz(b, a, N, h, w);
00131 
00132   return h;
00133 }
00134 
00135 
00136 cvec freqz(const vec &b, const vec &a, const vec &w)
00137 {
00138   int la = a.size(), lb = b.size(), k = std::max(la, lb);
00139 
00140   cvec h, ha, hb;
00141 
00142   // Evaluate the nominator and denominator at the given frequencies
00143   hb = polyval(zero_pad(b, k), to_cvec(cos(w), sin(w)));
00144   ha = polyval(zero_pad(a, k), to_cvec(cos(w), sin(w)));
00145 
00146   h = elem_div(hb, ha);
00147 
00148   return h;
00149 }
00150 
00151 
00152 
00153 void filter_design_autocorrelation(const int N, const vec &f, const vec &m, vec &R)
00154 {
00155   it_assert(f.size() == m.size(), "filter_design_autocorrelation: size of f and m vectors does not agree");
00156   int N_f = f.size();
00157 
00158   it_assert(f(0) == 0.0, "filter_design_autocorrelation: first frequency must be 0.0");
00159   it_assert(f(N_f - 1) == 1.0, "filter_design_autocorrelation: last frequency must be 1.0");
00160 
00161   // interpolate frequency-response
00162   int N_fft = 512;
00163   vec m_interp(N_fft + 1);
00164   // unused variable:
00165   // double df_interp = 1.0/double(N_fft);
00166 
00167   m_interp(0) = m(0);
00168   double inc;
00169 
00170   int jstart = 0, jstop;
00171 
00172   for (int i = 0; i < N_f - 1; i++) {
00173     // calculate number of points to the next frequency
00174     jstop = floor_i(f(i + 1) * (N_fft + 1)) - 1;
00175     //std::cout << "jstart=" << jstart << "jstop=" << jstop << std::endl;
00176 
00177     for (int j = jstart; j <= jstop; j++) {
00178       inc = double(j - jstart) / double(jstop - jstart);
00179       m_interp(j) = m(i) * (1 - inc) + m(i + 1) * inc;
00180     }
00181     jstart = jstop + 1;
00182   }
00183 
00184   vec S = sqr(concat(m_interp, reverse(m_interp(2, N_fft))));  // create a complete frequency response with also negative frequencies
00185 
00186   R = ifft_real(to_cvec(S)); // calculate correlation
00187 
00188   R = R.left(N);
00189 }
00190 
00191 
00192 // Calculate the AR coefficients of order \c n of the ARMA-process defined by the autocorrelation R
00193 // using the deternined modified Yule-Walker method
00194 // maxlag determines the size of the system to solve N>= n.
00195 // If N>m then the system is overdetermined and a least squares solution is used.
00196 // as a rule of thumb use N = 4*n
00197 void modified_yule_walker(const int m, const int n, const int N, const vec &R, vec &a)
00198 {
00199   it_assert(m > 0, "modified_yule_walker: m must be > 0");
00200   it_assert(n > 0, "modified_yule_walker: n must be > 0");
00201   it_assert(N <= R.size(), "modified_yule_walker: autocorrelation function too short");
00202 
00203   // create the modified Yule-Walker equations Rm * a = - rh
00204   // see eq. (3.7.1) in Stoica and Moses, Introduction to spectral analysis
00205   int M = N - m - 1;
00206 
00207   mat Rm;
00208   if (m - n + 1 < 0)
00209     Rm = toeplitz(R(m, m + M - 1), reverse(concat(R(1, std::abs(m - n + 1)), R(0, m))));
00210   else
00211     Rm = toeplitz(R(m, m + M - 1), reverse(R(m - n + 1, m)));
00212 
00213 
00214   vec rh = - R(m + 1, m + M);
00215 
00216   // solve overdetermined system
00217   a = backslash(Rm, rh);
00218 
00219   // prepend a_0 = 1
00220   a = concat(1.0, a);
00221 
00222   // stabilize polynomial
00223   a = polystab(a);
00224 }
00225 
00226 
00227 
00228 void arma_estimator(const int m, const int n, const vec &R, vec &b, vec &a)
00229 {
00230   it_assert(m > 0, "arma_estimator: m must be > 0");
00231   it_assert(n > 0, "arma_estimator: n must be > 0");
00232   it_assert(2*(m + n) <= R.size(), "arma_estimator: autocorrelation function too short");
00233 
00234 
00235   // windowing the autocorrelation
00236   int N = 2 * (m + n);
00237   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
00238 
00239   // calculate the AR part using the overdetmined Yule-Walker equations
00240   modified_yule_walker(m, n, N, Rwindow, a);
00241 
00242   // --------------- Calculate MA part --------------------------------------
00243   // use method in ref [2] section VII.
00244   vec r_causal = Rwindow;
00245   r_causal(0) *= 0.5;
00246 
00247   vec h_inv_a = filter(1, a, concat(1.0, zeros(N - 1))); // see eq (50) of [2]
00248   mat H_inv_a = toeplitz(h_inv_a, concat(1.0, zeros(m)));
00249 
00250   vec b_causal = backslash(H_inv_a, r_causal);
00251 
00252   // calculate the double-sided spectrum
00253   int N_fft = 256;
00254   vec H = 2.0 * real(elem_div(fft_real(b_causal, N_fft), fft_real(a, N_fft))); // calculate spectrum
00255 
00256   // Do weighting and windowing in cepstrum domain
00257   cvec cepstrum = log(to_cvec(H));
00258   cvec q = ifft(cepstrum);
00259 
00260   // keep only causal part of spectrum (windowing)
00261   q.set_subvector(N_fft / 2, zeros_c(N_fft / 2));
00262   q(0) *= 0.5;
00263 
00264   cvec h = ifft(exp(fft(q))); // convert back to frequency domain, from cepstrum and do inverse transform to calculate impulse response
00265   b = real(backslash(to_cmat(H_inv_a), h(0, N - 1))); // use Shank's method to calculate b coefficients
00266 }
00267 
00268 
00269 void yulewalk(const int N, const vec &f, const vec &m, vec &b, vec &a)
00270 {
00271   it_assert(f.size() == m.size(), "yulewalk: size of f and m vectors does not agree");
00272   int N_f = f.size();
00273 
00274   it_assert(f(0) == 0.0, "yulewalk: first frequency must be 0.0");
00275   it_assert(f(N_f - 1) == 1.0, "yulewalk: last frequency must be 1.0");
00276 
00277 
00278   vec R;
00279   filter_design_autocorrelation(4*N, f, m, R);
00280 
00281   arma_estimator(N, N, R, b, a);
00282 }
00283 
00284 
00285 } // namespace itpp
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines
SourceForge Logo

Generated on Sat Jul 9 2011 15:21:32 for IT++ by Doxygen 1.7.4