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
Generated on Sat Aug 25 23:37:27 2007 for IT++ by Doxygen 1.5.2