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