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
Generated on Sun Jul 26 08:54:30 2009 for IT++ by Doxygen 1.5.9