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 
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 Thu Apr 23 20:07:47 2009 for IT++ by Doxygen 1.5.8