Marsyas  0.5.0-beta1
/Users/jleben/code/marsyas/src/marsyas/marsystems/AimPZFC2.cpp
Go to the documentation of this file.
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 }