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
Generated on Sat Apr 19 11:01:27 2008 for IT++ by Doxygen 1.5.5