IT++ Logo Newcom Logo

filter_design.cpp

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

Generated on Wed Apr 18 11:23:29 2007 for IT++ by Doxygen 1.5.2