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

Generated on Sat Aug 25 23:40:02 2007 for IT++ by Doxygen 1.5.2