IT++ Logo

filter_design.cpp

Go to the documentation of this file.
00001 
00030 #include <itpp/signal/filter_design.h>
00031 #include <itpp/signal/poly.h>
00032 #include <itpp/signal/filter.h>
00033 #include <itpp/signal/transforms.h>
00034 #include <itpp/base/math/elem_math.h>
00035 #include <itpp/base/algebra/ls_solve.h>
00036 #include <itpp/base/matfunc.h>
00037 #include <itpp/base/specmat.h>
00038 #include <itpp/base/math/trig_hyp.h>
00039 #include <itpp/base/converters.h>
00040 
00041 
00042 namespace itpp {
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, N_fft-1, 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
SourceForge Logo

Generated on Sat Apr 19 10:57:52 2008 for IT++ by Doxygen 1.5.5