Marsyas
0.6.0-alpha
|
00001 /* 00002 ** Copyright (C) 1998-2010 George Tzanetakis <gtzan@cs.uvic.ca> 00003 ** 00004 ** This program is free software; you can redistribute it and/or modify 00005 ** it under the terms of the GNU General Public License as published by 00006 ** the Free Software Foundation; either version 2 of the License, or 00007 ** (at your option) any later version. 00008 ** 00009 ** This program is distributed in the hope that it will be useful, 00010 ** but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 ** GNU General Public License for more details. 00013 ** 00014 ** You should have received a copy of the GNU General Public License 00015 ** along with this program; if not, write to the Free Software 00016 ** Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 00017 */ 00018 00019 #include "AimPZFC.h" 00020 #include "../common_source.h" 00021 #include "../ERBTools.h" 00022 00023 using std::ostringstream; 00024 using std::cout; 00025 using std::endl; 00026 00027 using namespace Marsyas; 00028 00029 AimPZFC::AimPZFC(mrs_string name):MarSystem("AimPZFC",name) 00030 { 00031 is_initialized = false; 00032 initialized_israte = 0.0; 00033 initialized_inobservations = 0; 00034 initialized_mindamp = 0.0; 00035 initialized_maxdamp = 0.0; 00036 initialized_cf_max = 0.0; 00037 initialized_cf_min = 0.0; 00038 channel_count_ = 1; 00039 00040 is_reset = false; 00041 reseted_inobservations = 0; 00042 reseted_agc_factor = 0; 00043 00044 addControls(); 00045 } 00046 00047 AimPZFC::AimPZFC(const AimPZFC& a): MarSystem(a) 00048 { 00049 is_initialized = false; 00050 initialized_israte = 0.0; 00051 initialized_inobservations = 0; 00052 initialized_mindamp = 0.0; 00053 initialized_maxdamp = 0.0; 00054 initialized_cf_max = 0.0; 00055 initialized_cf_min = 0.0; 00056 00057 00058 channel_count_ = 1; 00059 00060 is_reset = false; 00061 reseted_inobservations = 0; 00062 reseted_agc_factor = 0; 00063 00064 ctrl_pole_damping_ = getctrl("mrs_real/pole_damping"); 00065 ctrl_zero_damping_ = getctrl("mrs_real/zero_damping"); 00066 ctrl_zero_factor_ = getctrl("mrs_real/zero_factor"); 00067 ctrl_step_factor_ = getctrl("mrs_real/step_factor"); 00068 ctrl_bandwidth_over_cf_ = getctrl("mrs_real/bandwidth_over_cf"); 00069 ctrl_min_bandwidth_hz_ = getctrl("mrs_real/min_bandwidth_hz"); 00070 ctrl_agc_factor_ = getctrl("mrs_real/agc_factor"); 00071 ctrl_cf_max_ = getctrl("mrs_real/cf_max"); 00072 ctrl_cf_min_ = getctrl("mrs_real/cf_min"); 00073 ctrl_mindamp_ = getctrl("mrs_real/mindamp"); 00074 ctrl_maxdamp_ = getctrl("mrs_real/maxdamp"); 00075 ctrl_do_agc_step_ = getctrl("mrs_bool/do_agc_step"); 00076 ctrl_use_fit_ = getctrl("mrs_bool/use_fit"); 00077 00078 } 00079 00080 00081 AimPZFC::~AimPZFC() 00082 { 00083 } 00084 00085 00086 MarSystem* 00087 AimPZFC::clone() const 00088 { 00089 return new AimPZFC(*this); 00090 } 00091 00092 void 00093 AimPZFC::addControls() 00094 { 00095 addControl("mrs_real/pole_damping", 0.12 , ctrl_pole_damping_); 00096 addControl("mrs_real/zero_damping", 0.2 , ctrl_zero_damping_); 00097 addControl("mrs_real/zero_factor", 1.4 , ctrl_zero_factor_); 00098 addControl("mrs_real/step_factor", 1.0/3.0 , ctrl_step_factor_); 00099 addControl("mrs_real/bandwidth_over_cf", 0.11 , ctrl_bandwidth_over_cf_); 00100 addControl("mrs_real/min_bandwidth_hz", 27.0 , ctrl_min_bandwidth_hz_); 00101 addControl("mrs_real/agc_factor", 12.0, ctrl_agc_factor_); 00102 addControl("mrs_real/cf_max", 6000.0, ctrl_cf_max_); 00103 addControl("mrs_real/cf_min", 100.0, ctrl_cf_min_); 00104 addControl("mrs_real/mindamp", 0.18 , ctrl_mindamp_); 00105 addControl("mrs_real/maxdamp", 0.4 , ctrl_maxdamp_); 00106 addControl("mrs_bool/do_agc_step", true , ctrl_do_agc_step_); 00107 addControl("mrs_bool/use_fit", false , ctrl_use_fit_); 00108 } 00109 00110 void 00111 AimPZFC::myUpdate(MarControlPtr sender) 00112 { 00113 00114 (void) sender; //suppress warning of unused parameter(s) 00115 MRSDIAG("AimPZFC.cpp - AimPZFC:myUpdate"); 00116 ctrl_onSamples_->setValue(ctrl_inSamples_, NOUPDATE); 00117 ctrl_osrate_->setValue(ctrl_israte_, NOUPDATE); 00118 ctrl_onObsNames_->setValue("AimPZFC_" + ctrl_inObsNames_->to<mrs_string>() , NOUPDATE); 00119 00120 // Need to have double the amount of channels, the first set of 00121 // channels are for the signals the second set of channels are for 00122 // the centre frequencies 00123 ctrl_onObservations_->setValue(channel_count_ * 2, NOUPDATE); 00124 00125 // 00126 // Does the MarSystem need initialization? 00127 // 00128 if (initialized_israte != ctrl_israte_->to<mrs_real>() || 00129 initialized_inobservations != ctrl_inObservations_->to<mrs_natural>() || 00130 initialized_mindamp != ctrl_mindamp_->to<mrs_real>() || 00131 initialized_maxdamp != ctrl_maxdamp_->to<mrs_real>() || 00132 initialized_cf_max != ctrl_cf_max_->to<mrs_real>() || 00133 initialized_cf_min != ctrl_cf_min_->to<mrs_real>()) { 00134 is_initialized = false; 00135 } 00136 00137 if (!is_initialized) { 00138 InitializeInternal(); 00139 is_initialized = true; 00140 initialized_israte = ctrl_israte_->to<mrs_real>(); 00141 initialized_inobservations = ctrl_inObservations_->to<mrs_natural>(); 00142 initialized_mindamp = ctrl_mindamp_->to<mrs_real>(); 00143 initialized_maxdamp = ctrl_maxdamp_->to<mrs_real>(); 00144 initialized_cf_max = ctrl_cf_max_->to<mrs_real>(); 00145 initialized_cf_min = ctrl_cf_min_->to<mrs_real>(); 00146 } 00147 00148 // 00149 // Does the MarSystem need a reset? 00150 // 00151 if (reseted_inobservations != ctrl_inObservations_->to<mrs_natural>() || 00152 reseted_agc_factor != ctrl_agc_factor_->to<mrs_real>()) { 00153 is_reset = false; 00154 } 00155 00156 if (!is_reset) { 00157 ResetInternal(); 00158 is_reset = true; 00159 reseted_inobservations = ctrl_inObservations_->to<mrs_natural>(); 00160 reseted_agc_factor = (mrs_natural) ctrl_agc_factor_->to<mrs_real>(); 00161 } 00162 00163 } 00164 00165 bool 00166 AimPZFC::InitializeInternal() { 00167 channel_count_ = 0; 00168 SetPZBankCoeffs(); 00169 return true; 00170 } 00171 00172 void 00173 AimPZFC::ResetInternal() { 00174 // cout << "AimPZFC::ResetInternal" << endl; 00175 00176 // These buffers may be actively modified by the algorithm 00177 agc_state_.clear(); 00178 agc_state_.resize(channel_count_); 00179 for (int i = 0; i < channel_count_; ++i) { 00180 agc_state_[i].clear(); 00181 agc_state_[i].resize(agc_stage_count_, 0.0); 00182 } 00183 00184 state_1_.clear(); 00185 state_1_.resize(channel_count_, 0.0); 00186 00187 state_2_.clear(); 00188 state_2_.resize(channel_count_, 0.0); 00189 00190 previous_out_.clear(); 00191 previous_out_.resize(channel_count_, 0.0); 00192 00193 pole_damps_mod_.clear(); 00194 pole_damps_mod_.resize(channel_count_, 0.0); 00195 00196 inputs_.clear(); 00197 inputs_.resize(channel_count_, 0.0); 00198 00199 // Init AGC 00200 offset_ = 1.0 - ctrl_agc_factor_->to<mrs_real>() * DetectFun(0.0); 00201 agc_factor_ = ctrl_agc_factor_->to<mrs_real>(); 00202 AGCDampStep(); 00203 // pole_damps_mod_ and agc_state_ are now be initialized 00204 00205 // Modify the pole dampings and AGC state slightly from their values in 00206 // silence in case the input is abuptly loud. 00207 for (int i = 0; i < channel_count_; ++i) { 00208 pole_damps_mod_[i] += 0.05; 00209 for (int j = 0; j < agc_stage_count_; ++j) 00210 agc_state_[i][j] += 0.05; 00211 } 00212 00213 last_input_ = 0.0; 00214 } 00215 00216 bool 00217 AimPZFC::SetPZBankCoeffs() { 00220 if (ctrl_use_fit_->to<mrs_bool>()) { 00221 if (!SetPZBankCoeffsERBFitted()) 00222 return false; 00223 } else { 00224 if (!SetPZBankCoeffsOrig()) 00225 return false; 00226 } 00227 00228 double mindamp = ctrl_mindamp_->to<mrs_real>(); 00229 double maxdamp = ctrl_maxdamp_->to<mrs_real>(); 00230 00231 rmin_.resize(channel_count_); 00232 rmax_.resize(channel_count_); 00233 xmin_.resize(channel_count_); 00234 xmax_.resize(channel_count_); 00235 00236 for (int c = 0; c < channel_count_; ++c) { 00237 // Calculate maximum and minimum damping options 00238 rmin_[c] = exp(-mindamp * pole_frequencies_[c]); 00239 rmax_[c] = exp(-maxdamp * pole_frequencies_[c]); 00240 00241 xmin_[c] = rmin_[c] * cos(pole_frequencies_[c] 00242 * pow((1-pow(mindamp, 2)), 0.5)); 00243 xmax_[c] = rmax_[c] * cos(pole_frequencies_[c] 00244 * pow((1-pow(maxdamp, 2)), 0.5)); 00245 00246 } 00247 00248 // Set up AGC parameters 00249 agc_stage_count_ = 4; 00250 agc_epsilons_.resize(agc_stage_count_); 00251 agc_epsilons_[0] = 0.0064; 00252 agc_epsilons_[1] = 0.0016; 00253 agc_epsilons_[2] = 0.0004; 00254 agc_epsilons_[3] = 0.0001; 00255 00256 agc_gains_.resize(agc_stage_count_); 00257 agc_gains_[0] = 1.0; 00258 agc_gains_[1] = 1.4; 00259 agc_gains_[2] = 2.0; 00260 agc_gains_[3] = 2.8; 00261 00262 double mean_agc_gain = 0.0; 00263 for (int c = 0; c < agc_stage_count_; ++c) 00264 mean_agc_gain += agc_gains_[c]; 00265 mean_agc_gain /= static_cast<double>(agc_stage_count_); 00266 00267 for (int c = 0; c < agc_stage_count_; ++c) 00268 agc_gains_[c] /= mean_agc_gain; 00269 00270 return true; 00271 } 00272 00273 bool 00274 AimPZFC::SetPZBankCoeffsOrig() { 00275 // This function sets the following variables: 00276 // channel_count_ 00277 // pole_dampings_ 00278 // pole_frequencies_ 00279 // za0_, za1_, za2 00280 // output_ 00281 00282 double sample_rate = getctrl("mrs_real/israte")->to<mrs_real>(); 00283 double cf_max = getctrl("mrs_real/cf_max")->to<mrs_real>(); 00284 double cf_min = getctrl("mrs_real/cf_min")->to<mrs_real>(); 00285 double bandwidth_over_cf = getctrl("mrs_real/bandwidth_over_cf")->to<mrs_real>(); 00286 double min_bandwidth_hz = getctrl("mrs_real/min_bandwidth_hz")->to<mrs_real>(); 00287 double step_factor = getctrl("mrs_real/step_factor")->to<mrs_real>(); 00288 double pole_damping = getctrl("mrs_real/pole_damping")->to<mrs_real>(); 00289 double zero_factor = getctrl("mrs_real/zero_factor")->to<mrs_real>(); 00290 double zero_damping = getctrl("mrs_real/zero_damping")->to<mrs_real>(); 00291 00292 // TODO(tomwalters): There's significant code-duplication between this function 00293 // and SetPZBankCoeffsERBFitted, and SetPZBankCoeffs 00294 00295 // Normalised maximum pole frequency 00296 double pole_frequency = cf_max / sample_rate * (2.0 * PI); 00297 channel_count_ = 0; 00298 while ((pole_frequency / (2.0 * PI)) * sample_rate > cf_min) { 00299 double bw = bandwidth_over_cf * pole_frequency + 2 * PI * min_bandwidth_hz / sample_rate; 00300 pole_frequency -= step_factor * bw; 00301 channel_count_++; 00302 } 00303 00304 00305 00306 // Now the number of channels is known, various buffers for the filterbank 00307 // coefficients can be initialised 00308 pole_dampings_.clear(); 00309 pole_dampings_.resize(channel_count_, pole_damping); 00310 pole_frequencies_.clear(); 00311 pole_frequencies_.resize(channel_count_, 0.0); 00312 00313 // Direct-form coefficients 00314 za0_.clear(); 00315 za0_.resize(channel_count_, 0.0); 00316 za1_.clear(); 00317 za1_.resize(channel_count_, 0.0); 00318 za2_.clear(); 00319 za2_.resize(channel_count_, 0.0); 00320 00321 // The output signal bank 00322 // output_.Initialize(channel_count_, buffer_length_, sample_rate); 00323 00324 // cout.precision(20); 00325 00326 // cout << "********** 1/3=" << 1.0/3.0 << endl; 00327 00328 // Reset the pole frequency to maximum 00329 pole_frequency = cf_max / sample_rate * (2.0 * PI); 00330 // cout << "cf_max=" << cf_max << endl; 00331 // cout << "sample_rate=" << sample_rate << endl; 00332 // cout << "pole_frequency=" << pole_frequency << endl; 00333 00334 centre_frequencies_.clear(); 00335 centre_frequencies_.resize(channel_count_); 00336 00337 for (int i = channel_count_ - 1; i > -1; --i) { 00338 // cout << "i=" << i << endl; 00339 00340 // Store the normalised pole frequncy 00341 pole_frequencies_[i] = pole_frequency; 00342 00343 // Calculate the real pole frequency from the normalised pole frequency 00344 double frequency = pole_frequency / (2.0 * PI) * sample_rate; 00345 00346 // Store the real pole frequency as the 'centre frequency' of the filterbank 00347 // channel 00348 centre_frequencies_[i] = frequency; 00349 // output_.set_centre_frequency(i, frequency); 00350 00351 double zero_frequency = Minimum(PI, zero_factor * pole_frequency); 00352 // cout << "\tzero_frequency=" << zero_frequency << endl; 00353 00354 // Impulse-invariance mapping 00355 double z_plane_theta = zero_frequency * sqrt(1.0 - pow(zero_damping, 2)); 00356 double z_plane_rho = exp(-zero_damping * zero_frequency); 00357 // cout << "\tz_plane_theta=" << z_plane_theta << endl; 00358 // cout << "\tz_plane_rho=" << z_plane_rho << endl; 00359 00360 // Direct-form coefficients from z-plane rho and theta 00361 double a1 = -2.0 * z_plane_rho * cos(z_plane_theta); 00362 double a2 = z_plane_rho * z_plane_rho; 00363 00364 // Normalised to unity gain at DC 00365 double a_sum = 1.0 + a1 + a2; 00366 // cout << "\ta1=" << a1 << endl; 00367 // cout << "\ta2=" << a2 << endl; 00368 // cout << "\ta_sum=" << a_sum << endl; 00369 00370 za0_[i] = 1.0 / a_sum; 00371 za1_[i] = a1 / a_sum; 00372 za2_[i] = a2 / a_sum; 00373 00374 // Subtract step factor (1/n2) times current bandwidth from the pole 00375 // frequency 00376 double bw = bandwidth_over_cf * pole_frequency + 2 * PI * min_bandwidth_hz / sample_rate; 00377 // cout << "\tmin_bandwidth_hz_=" << min_bandwidth_hz << endl; 00378 // cout << "\tbw=" << bw << endl; 00379 // cout << "\tstep_factor=" << step_factor << endl; 00380 pole_frequency -= step_factor * bw; 00381 } 00382 00383 return true; 00384 } 00385 00386 00387 // bool 00388 // AimPZFC::SetPZBankCoeffsERB() { 00389 // // This function sets the following variables: 00390 // // channel_count_ 00391 // // pole_dampings_ 00392 // // pole_frequencies_ 00393 // // za0_, za1_, za2 00394 // // output_ 00395 00396 // double sample_rate = getctrl("mrs_real/israte")->to<mrs_real>(); 00397 // double cf_max = getctrl("mrs_real/cf_max")->to<mrs_real>(); 00398 // double cf_min = getctrl("mrs_real/cf_min")->to<mrs_real>(); 00399 // double bandwidth_over_cf = getctrl("mrs_real/bandwidth_over_cf")->to<mrs_real>(); 00400 // double min_bandwidth_hz = getctrl("mrs_real/min_bandwidth_hz")->to<mrs_real>(); 00401 // double step_factor = getctrl("mrs_real/step_factor")->to<mrs_real>(); 00402 // double pole_damping = getctrl("mrs_real/pole_damping")->to<mrs_real>(); 00403 // double zero_factor = getctrl("mrs_real/zero_factor")->to<mrs_real>(); 00404 // double zero_damping = getctrl("mrs_real/zero_damping")->to<mrs_real>(); 00405 00406 // // TODO(tomwalters): There's significant code-duplication between here, 00407 // // SetPZBankCoeffsERBFitted, and SetPZBankCoeffs 00408 00409 // // Normalised maximum pole frequency 00410 // double pole_frequency = cf_max / sample_rate * (2.0 * PI); 00411 // channel_count_ = 0; 00412 // while ((pole_frequency / (2.0 * PI)) * sample_rate > cf_min) { 00413 // double bw = ERBTools::Freq2ERBw(pole_frequency 00414 // / (2.0 * PI) * sample_rate); 00415 // pole_frequency -= step_factor * (bw * (2.0 * PI) / sample_rate); 00416 // channel_count_++; 00417 // } 00418 00419 // // Now the number of channels is known, various buffers for the filterbank 00420 // // coefficients can be initialised 00421 // pole_dampings_.clear(); 00422 // pole_dampings_.resize(channel_count_, pole_damping); 00423 // pole_frequencies_.clear(); 00424 // pole_frequencies_.resize(channel_count_, 0.0); 00425 00426 // // Direct-form coefficients 00427 // za0_.clear(); 00428 // za0_.resize(channel_count_, 0.0); 00429 // za1_.clear(); 00430 // za1_.resize(channel_count_, 0.0); 00431 // za2_.clear(); 00432 // za2_.resize(channel_count_, 0.0); 00433 00434 // // The output signal bank 00435 // // output_.Initialize(channel_count_, buffer_length_, sample_rate); 00436 00437 // // Reset the pole frequency to maximum 00438 // pole_frequency = cf_max / sample_rate * (2.0 * PI); 00439 00440 // for (int i = channel_count_ - 1; i > -1; --i) { 00441 // // Store the normalised pole frequncy 00442 // pole_frequencies_[i] = pole_frequency; 00443 00444 // // Calculate the real pole frequency from the normalised pole frequency 00445 // double frequency = pole_frequency / (2.0 * PI) * sample_rate; 00446 00447 // // Store the real pole frequency as the 'centre frequency' of the filterbank 00448 // // channel 00449 // // output_.set_centre_frequency(i, frequency); 00450 00451 // double zero_frequency = Minimum(PI, zero_factor * pole_frequency); 00452 00453 // // Impulse-invariance mapping 00454 // double z_plane_theta = zero_frequency * sqrt(1.0 - pow(zero_damping, 2)); 00455 // double z_plane_rho = exp(-zero_damping * zero_frequency); 00456 00457 // // Direct-form coefficients from z-plane rho and theta 00458 // double a1 = -2.0 * z_plane_rho * cos(z_plane_theta); 00459 // double a2 = z_plane_rho * z_plane_rho; 00460 00461 // // Normalised to unity gain at DC 00462 // double a_sum = 1.0 + a1 + a2; 00463 // za0_[i] = 1.0 / a_sum; 00464 // za1_[i] = a1 / a_sum; 00465 // za2_[i] = a2 / a_sum; 00466 00467 // double bw = ERBTools::Freq2ERBw(pole_frequency 00468 // / (2.0 * PI) * sample_rate); 00469 // pole_frequency -= step_factor * (bw * (2.0 * PI) / sample_rate); 00470 // } 00471 // return true; 00472 // } 00473 00474 bool 00475 AimPZFC::SetPZBankCoeffsERBFitted() { 00476 // cout << "ModulePZFC::SetPZBankCoeffsERBFitted" << endl; 00477 // cout << "AimPZFC::SetPZBankCoeffsERBFitted" << endl; 00478 //double parameter_values[3 * 7] = { 00480 //1.14827, 0.00000, 0.00000, // % SumSqrErr= 10125.41 00481 //0.53571, -0.70128, 0.63246, // % RMSErr = 2.81586 00482 //0.76779, 0.00000, 0.00000, // % MeanErr = 0.00000 00484 //0.00000, 0.00000, 0.00000, 00485 //6.00000, 0.00000, 0.00000, 00486 //1.08869, -0.09470, 0.07844, 00487 //10.56432, 2.52732, 1.86895 00489 //}; 00490 00491 double parameter_values[3 * 7] = { 00492 // Fit 515 from Dick 00493 // Final, Nfit = 515, 9-3 parameters, PZFC, cwt 0 00494 1.72861, 0.00000, 0.00000, // SumSqrErr = 13622.24 00495 0.56657, -0.93911, 0.89163, // RMSErr = 3.26610 00496 0.39469, 0.00000, 0.00000, // MeanErr = 0.00000 00497 // Inf, 0.00000, 0.00000, // RMSCost = NaN - would set coefc to infinity, but this isn't passed on 00498 0.00000, 0.00000, 0.00000, 00499 2.00000, 0.00000, 0.00000, // 00500 1.27393, 0.00000, 0.00000, 00501 11.46247, 5.46894, 0.11800 00502 // -4.15525, 1.54874, 2.99858 // Kv 00503 }; 00504 00505 double sample_rate = getctrl("mrs_real/israte")->to<mrs_real>(); 00506 double cf_max = getctrl("mrs_real/cf_max")->to<mrs_real>(); 00507 double cf_min = getctrl("mrs_real/cf_min")->to<mrs_real>(); 00508 00509 // Precalculate the number of channels required - this method is ugly but it 00510 // was the quickest way of converting from MATLAB as the step factor between 00511 // channels can vary quadratically with pole frequency... 00512 00513 // Normalised maximum pole frequency 00514 double pole_frequency = cf_max / sample_rate * (2.0 * PI); 00515 00516 channel_count_ = 0; 00517 while ((pole_frequency / (2.0 * PI)) * sample_rate > cf_min) { 00518 double frequency = pole_frequency / (2.0 * PI) * sample_rate; 00519 double f_dep = ERBTools::Freq2ERB(frequency) 00520 / ERBTools::Freq2ERB(1000.0) - 1.0; 00521 double bw = ERBTools::Freq2ERBw(pole_frequency 00522 / (2.0 * PI) * sample_rate); 00523 double step_factor = 1.0 00524 / (parameter_values[4*3] + parameter_values[4 * 3 + 1] 00525 * f_dep + parameter_values[4 * 3 + 2] * f_dep * f_dep); // 1/n2 00526 pole_frequency -= step_factor * (bw * (2.0 * PI) / sample_rate); 00527 channel_count_++; 00528 } 00529 00530 // Now the number of channels is known, various buffers for the filterbank 00531 // coefficients can be initialised 00532 pole_dampings_.clear(); 00533 pole_dampings_.resize(channel_count_, 0.0); 00534 pole_frequencies_.clear(); 00535 pole_frequencies_.resize(channel_count_, 0.0); 00536 00537 // Direct-form coefficients 00538 za0_.clear(); 00539 za0_.resize(channel_count_, 0.0); 00540 za1_.clear(); 00541 za1_.resize(channel_count_, 0.0); 00542 za2_.clear(); 00543 za2_.resize(channel_count_, 0.0); 00544 00545 // The output signal bank 00546 // output_.Initialize(channel_count_, buffer_length_, sample_rate); 00547 00548 // Reset the pole frequency to maximum 00549 pole_frequency = cf_max / sample_rate * (2.0 * PI); 00550 00551 for (int i = channel_count_ - 1; i > -1; --i) { 00552 // Store the normalised pole frequncy 00553 pole_frequencies_[i] = pole_frequency; 00554 00555 // Calculate the real pole frequency from the normalised pole frequency 00556 double frequency = pole_frequency / (2.0 * PI) * sample_rate; 00557 00558 // Store the real pole frequency as the 'centre frequency' of the filterbank 00559 // channel 00560 // output_.set_centre_frequency(i, frequency); 00561 00562 // From PZFC_Small_Signal_Params.m { From PZFC_Params.m { 00563 double DpndF = ERBTools::Freq2ERB(frequency) 00564 / ERBTools::Freq2ERB(1000.0) - 1.0; 00565 00566 double p[8]; // Parameters (short name for ease of reading) 00567 00568 // Use parameter_values to recover the parameter values for this frequency 00569 for (int param = 0; param < 7; ++param) 00570 p[param] = parameter_values[param * 3] 00571 + parameter_values[param * 3 + 1] * DpndF 00572 + parameter_values[param * 3 + 2] * DpndF * DpndF; 00573 00574 // Calculate the final parameter 00575 p[7] = p[1] * pow(10.0, (p[2] / (p[1] * p[4])) * (p[6] - 60.0) / 20.0); 00576 if (p[7] < 0.2) 00577 p[7] = 0.2; 00578 00579 // Nominal bandwidth at this frequency 00580 double fERBw = ERBTools::Freq2ERBw(frequency); 00581 00582 // Pole bandwidth 00583 double fPBW = ((p[7] * fERBw * (2 * PI) / sample_rate) / 2) 00584 * pow(p[4], 0.5); 00585 00586 // Pole damping 00587 double pole_damping = fPBW / sqrt(pow(pole_frequency, 2) + pow(fPBW, 2)); 00588 00589 cout << "pole_damping = " << pole_damping << endl; 00590 00591 // Store the pole damping 00592 pole_dampings_[i] = pole_damping; 00593 00594 // Zero bandwidth 00595 double fZBW = ((p[0] * p[5] * fERBw * (2 * PI) / sample_rate) / 2) 00596 * pow(p[4], 0.5); 00597 00598 // Zero frequency 00599 double zero_frequency = p[5] * pole_frequency; 00600 00601 if (zero_frequency > PI) { 00602 MRSWARN("Warning: Zero frequency is above the Nyquist frequency."); 00603 MRSWARN("Continuing anyway but results may not be accurate."); 00604 } 00605 // LOG_ERROR(_T("Warning: Zero frequency is above the Nyquist frequency " 00606 // "in ModulePZFC(), continuing anyway but results may not " 00607 // "be accurate.")); 00608 00609 // Zero damping 00610 double fZDamp = fZBW / sqrt(pow(zero_frequency, 2) + pow(fZBW, 2)); 00611 00612 // Impulse-invariance mapping 00613 double fZTheta = zero_frequency * sqrt(1.0 - pow(fZDamp, 2)); 00614 double fZRho = exp(-fZDamp * zero_frequency); 00615 00616 // Direct-form coefficients 00617 double fA1 = -2.0 * fZRho * cos(fZTheta); 00618 double fA2 = fZRho * fZRho; 00619 00620 // Normalised to unity gain at DC 00621 double fASum = 1.0 + fA1 + fA2; 00622 za0_[i] = 1.0 / fASum; 00623 za1_[i] = fA1 / fASum; 00624 za2_[i] = fA2 / fASum; 00625 00626 // Subtract step factor (1/n2) times current bandwidth from the pole 00627 // frequency 00628 pole_frequency -= ((1.0 / p[4]) 00629 * (fERBw * (2.0 * PI) / sample_rate)); 00630 } 00631 return true; 00632 } 00633 00634 void 00635 AimPZFC::AGCDampStep() { 00636 // cout << "AimPZFC::AGCDampStep" << endl; 00637 if (detect_.size() == 0) { 00638 // If detect_ is not initialised, it means that the AGC is not set up. 00639 // Set up now. 00642 detect_.clear(); 00643 double detect_zero = DetectFun(0.0); 00644 detect_.resize(channel_count_, detect_zero); 00645 00646 for (int c = 0; c < channel_count_; c++) 00647 for (int st = 0; st < agc_stage_count_; st++) 00648 agc_state_[c][st] = (1.2 * detect_[c] * agc_gains_[st]); 00649 } 00650 00651 double fAGCEpsLeft = 0.3; 00652 double fAGCEpsRight = 0.3; 00653 00654 for (int c = channel_count_ - 1; c > -1; --c) { 00655 for (int st = 0; st < agc_stage_count_; ++st) { 00656 // This bounds checking is ugly and wasteful, and in an inner loop. 00657 // If this algorithm is slow, this is why! 00660 double fPrevAGCState; 00661 double fCurrAGCState; 00662 double fNextAGCState; 00663 00664 if (c < channel_count_ - 1) 00665 fPrevAGCState = agc_state_[c + 1][st]; 00666 else 00667 fPrevAGCState = agc_state_[c][st]; 00668 00669 fCurrAGCState = agc_state_[c][st]; 00670 00671 if (c > 0) 00672 fNextAGCState = agc_state_[c - 1][st]; 00673 else 00674 fNextAGCState = agc_state_[c][st]; 00675 00676 // Spatial smoothing 00680 double agc_avg = fAGCEpsLeft * fPrevAGCState 00681 + (1.0 - fAGCEpsLeft - fAGCEpsRight) * fCurrAGCState 00682 + fAGCEpsRight * fNextAGCState; 00683 // Temporal smoothing 00684 agc_state_[c][st] = agc_avg * (1.0 - agc_epsilons_[st]) 00685 + agc_epsilons_[st] * detect_[c] * agc_gains_[st]; 00686 } 00687 } 00688 00689 // double offset = 1.0 - agc_factor_ * DetectFun(0.0); 00690 00691 00692 for (int i = 0; i < channel_count_; ++i) { 00693 double fAGCStateMean = 0.0; 00694 for (int j = 0; j < agc_stage_count_; ++j) 00695 fAGCStateMean += agc_state_[i][j]; 00696 00697 fAGCStateMean /= static_cast<double>(agc_stage_count_); 00698 00699 00700 pole_damps_mod_[i] = pole_dampings_[i] * 00701 (offset_ + agc_factor_ * fAGCStateMean); 00702 } 00703 } 00704 00705 double 00706 AimPZFC::DetectFun(double fIN) 00707 { 00708 if (fIN < 0.0) 00709 fIN = 0.0; 00710 double fDetect = Minimum(1.0, fIN); 00711 double fA = 0.25; 00712 return fA * fIN + (1.0 - fA) * (fDetect - pow(fDetect, 3) / 3.0); 00713 } 00714 00715 inline double AimPZFC::Minimum(double a, double b) 00716 { 00717 if (a < b) 00718 return a; 00719 else 00720 return b; 00721 } 00722 00723 00724 void 00725 AimPZFC::myProcess(realvec& in, realvec& out) 00726 { 00727 double damp_rate = 1.0 / (ctrl_maxdamp_->to<mrs_real>() - ctrl_mindamp_->to<mrs_real>()); 00728 double min_damp = ctrl_mindamp_->to<mrs_real>(); 00729 double interp_factor; 00730 bool do_agc = ctrl_do_agc_step_->to<mrs_bool>(); 00731 00732 for (mrs_natural t = 0; t < inSamples_; t++) 00733 { 00734 double input_sample = in(0, t); 00735 00736 // Lowpass filter the input with a zero at PI 00737 input_sample = 0.5 * input_sample + 0.5 * last_input_; 00738 last_input_ = in(0, t); 00739 00740 inputs_[channel_count_ - 1] = input_sample; 00741 for (int c = 0; c < channel_count_ - 1; ++c) 00742 { 00743 inputs_[c] = previous_out_[c + 1]; 00744 00745 } 00746 00747 // PZBankStep2 00748 // to save a bunch of divides 00749 for (int c = channel_count_ - 1; c > -1; --c) 00750 { 00751 interp_factor = (pole_damps_mod_[c] -min_damp) * damp_rate; 00752 00753 double x = xmin_[c] + (xmax_[c] - xmin_[c]) * interp_factor; 00754 double r = rmin_[c] + (rmax_[c] - rmin_[c]) * interp_factor; 00755 00756 // optional improvement to constellation adds a bit to r 00757 double fd = pole_frequencies_[c] * pole_damps_mod_[c]; 00758 // quadratic for small values, then linear 00759 r = r + 0.25 * fd * Minimum(0.05, fd); 00760 00761 double zb1 = -2.0 * x; 00762 double zb2 = r * r; 00763 00764 // canonic poles but with input provided where unity DC gain is 00765 // assured (mean value of state is always equal to mean value 00766 // of input) 00767 double new_state = inputs_[c] - (state_1_[c] - inputs_[c]) * zb1 00768 - (state_2_[c] - inputs_[c]) * zb2; 00769 00770 // canonic zeros part as before: 00771 // cout << "za0_[c]=" << za0_[c] << endl; 00772 // cout << "new_state=" << new_state << endl; 00773 // cout << "za1_[c]=" << za1_[c] << endl; 00774 // cout << "state_1_[c]=" << state_1_[c] << endl; 00775 // cout << "state_2_[c]=" << state_2_[c] << endl; 00776 00777 double output = za0_[c] * new_state + za1_[c] * state_1_[c] 00778 + za2_[c] * state_2_[c]; 00779 00780 // cubic compression nonlinearity 00781 output -= 0.0001 * pow(output, 3); 00782 // cout << "output=" << output << endl; 00783 00784 out(c, t) = output; 00785 detect_[c] = DetectFun(output); 00786 state_2_[c] = state_1_[c]; 00787 state_1_[c] = new_state; 00788 } 00789 00790 if (do_agc) 00791 { 00792 // double offset = 1.0 - ctrl_agc_factor_->to<mrs_real>() * DetectFun(0.0); 00793 // double agc_factor = ctrl_agc_factor_->to<mrs_real>(); 00794 AGCDampStep(); 00795 } 00796 00797 for (int c = 0; c < channel_count_; ++c) 00798 previous_out_[c] = out(c,t); 00799 } 00800 00801 00802 // Copy over the centre frequencies to the second half of the observations 00803 for (mrs_natural t = 0; t < inSamples_; t++) 00804 { 00805 for (mrs_natural o = 0; o < channel_count_; o++) 00806 { 00807 out(o + channel_count_, t) = centre_frequencies_[o]; 00808 } 00809 } 00810 00811 }