Marsyas  0.6.0-alpha
/usr/src/RPM/BUILD/marsyas-0.6.0/src/marsyas/marsystems/AimPZFC.cpp
Go to the documentation of this file.
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 }