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 00042 00043 namespace itpp { 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 Wed Apr 18 11:23:29 2007 for IT++ by Doxygen 1.5.2