diff src/Modules/BMM/ModulePZFC.cc @ 0:582cbe817f2c

- Initial add of support code and modules. Not everything is working yet.

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Modules/BMM/ModulePZFC.cc	Fri Feb 12 12:31:23 2010 +0000
@@ -0,0 +1,453 @@
+// Copyright 2008-2010, Thomas Walters
+//
+// AIM-C: A C++ implementation of the Auditory Image Model
+// http://www.acousticscale.org/AIMC
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+/*! \file
+ *  \brief Dick Lyon's Pole-Zero Filter Cascade - implemented as an AIM-C
+ *  module by Tom Walters from the AIM-MAT module based on Dick Lyon's code
+ */
+
+/*! \author Thomas Walters <[e-mail]>
+ *  \date created 2008/02/05
+ *  \version \$Id: ModulePZFC.cc 4 2010-02-03 18:44:58Z tcw $
+ */
+
+#include "Support/ERBTools.h"
+
+#include "Modules/BMM/ModulePZFC.h"
+
+namespace aimc {
+ModulePZFC::ModulePZFC(Parameters *parameters) : Module(parameters) {
+  module_identifier_ = "pzfc";
+  module_type_ = "bmm";
+  module_description_ = "Pole-Zero Filter Cascade";
+  module_version_ = "$Id: ModulePZFC.cc 4 2010-02-03 18:44:58Z tcw $";
+
+  // Get parameter values, setting default values where necessary
+  // Each parameter is set here only if it has not already been set elsewhere.
+  cf_max_ = parameters_->DefaultFloat("pzfc.highest_frequency", 6000.0f);
+  cf_min_ = parameters_->DefaultFloat("pzfc.lowest_frequency", 100.0f);
+  pole_damping_ = parameters_->DefaultFloat("pzfc.pole_damping", 0.12f);
+  zero_damping_ = parameters_->DefaultFloat("pzfc.zero_damping", 0.2f);
+  zero_factor_ = parameters_->DefaultFloat("pzfc.zero_factor", 1.4f);
+  step_factor_ = parameters_->DefaultFloat("pzfc.step_factor", 1.0f/3.0f);
+  bandwidth_over_cf_ = parameters_->DefaultFloat("pzfc.bandwidth_over_cf",
+                                                 0.11f);
+  min_bandwidth_hz_ = parameters_->DefaultFloat("pzfc.min_bandwidth_hz",
+                                                27.0f);
+  agc_factor_ = parameters_->DefaultFloat("pzfc.agc_factor", 12.0f);
+  do_agc_step_ = parameters_->DefaultBool("pzfc.do_agc", true);
+
+  detect_.resize(0);
+}
+
+ModulePZFC::~ModulePZFC() {
+}
+
+bool ModulePZFC::InitializeInternal(const SignalBank &input) {
+  // Make local convenience copies of some variables
+  sample_rate_ = input.sample_rate();
+  buffer_length_ = input.buffer_length();
+  channel_count_ = 0;
+
+  // Prepare the coefficients and also the output SignalBank
+  if (!SetPZBankCoeffs())
+    return false;
+
+  // The output signal bank should be set up by now.
+  if (!output_.initialized())
+    return false;
+
+  // This initialises all buffers which can be modified by Process()
+  Reset();
+
+  return true;
+}
+
+void ModulePZFC::Reset() {
+  // These buffers may be actively modified by the algorithm
+  agc_state_.clear();
+  agc_state_.resize(channel_count_);
+  for (int i = 0; i < channel_count_; ++i) {
+    agc_state_[i].clear();
+    agc_state_[i].resize(agc_stage_count_, 0.0f);
+  }
+
+  state_1_.clear();
+  state_1_.resize(channel_count_, 0.0f);
+
+  state_2_.clear();
+  state_2_.resize(channel_count_, 0.0f);
+
+  previous_out_.clear();
+  previous_out_.resize(channel_count_, 0.0f);
+
+  pole_damps_mod_.clear();
+  pole_damps_mod_.resize(channel_count_, 0.0f);
+
+  inputs_.clear();
+  inputs_.resize(channel_count_, 0.0f);
+
+  // Init AGC
+  AGCDampStep();
+  // pole_damps_mod_ and agc_state_ are now be initialized
+
+  // Modify the pole dampings and AGC state slightly from their values in
+  // silence in case the input is abuptly loud.
+  for (int i = 0; i < channel_count_; ++i) {
+    pole_damps_mod_[i] += 0.05f;
+    for (int j = 0; j < agc_stage_count_; ++j)
+      agc_state_[i][j] += 0.05f;
+  }
+
+  last_input_ = 0.0f;
+}
+
+bool ModulePZFC::SetPZBankCoeffsERBFitted() {
+  float parameter_values[3 * 7] = {
+    // Filed, Nfit = 524, 11-3 parameters, PZFC, cwt 0, fit time 9915 sec
+    1.14827,   0.00000,   0.00000,  // % SumSqrErr=  10125.41
+    0.53571,  -0.70128,   0.63246,  // % RMSErr   =   2.81586
+    0.76779,   0.00000,   0.00000,  // % MeanErr  =   0.00000
+    // Inf   0.00000   0.00000 % RMSCost  = NaN
+    0.00000,   0.00000,   0.00000,
+    6.00000,   0.00000,   0.00000,
+    1.08869,  -0.09470,   0.07844,
+    10.56432,   2.52732,   1.86895
+    // -3.45865  -1.31457   3.91779 % Kv
+  };
+
+  // Precalculate the number of channels required - this method is ugly but it
+  // was the quickest way of converting from MATLAB as the step factor between
+  // channels can vary quadratically with pole frequency...
+
+  // Normalised maximum pole frequency
+  float pole_frequency = cf_max_ / sample_rate_ * (2.0f * M_PI);
+
+  channel_count_ = 0;
+  while ((pole_frequency / (2.0f * M_PI)) * sample_rate_ > cf_min_) {
+    float frequency = pole_frequency / (2.0f * M_PI) * sample_rate_;
+    float f_dep = ERBTools::Freq2ERB(frequency)
+                  / ERBTools::Freq2ERB(1000.0f) - 1.0f;
+    float bw = ERBTools::Freq2ERBw(pole_frequency
+                                  / (2.0f * M_PI) * sample_rate_);
+    float step_factor = 1.0f
+      / (parameter_values[4*3] + parameter_values[4 * 3 + 1]
+      * f_dep + parameter_values[4 * 3 + 2] * f_dep * f_dep);  // 1/n2
+    pole_frequency -= step_factor * (bw * (2.0f * M_PI) / sample_rate_);
+    channel_count_++;
+  }
+
+  // Now the number of channels is known, various buffers for the filterbank
+  // coefficients can be initialised
+  pole_dampings_.clear();
+  pole_dampings_.resize(channel_count_, 0.0f);
+  pole_frequencies_.clear();
+  pole_frequencies_.resize(channel_count_, 0.0f);
+
+  // Direct-form coefficients
+  za0_.clear();
+  za0_.resize(channel_count_, 0.0f);
+  za1_.clear();
+  za1_.resize(channel_count_, 0.0f);
+  za2_.clear();
+  za2_.resize(channel_count_, 0.0f);
+
+  // The output signal bank
+  output_.Initialize(channel_count_, buffer_length_, sample_rate_);
+
+  // Reset the pole frequency to maximum
+  pole_frequency = cf_max_ / sample_rate_ * (2.0f * M_PI);
+
+  for (int i = channel_count_ - 1; i > -1; --i) {
+    // Store the normalised pole frequncy
+    pole_frequencies_[i] = pole_frequency;
+
+    // Calculate the real pole frequency from the normalised pole frequency
+    float frequency = pole_frequency / (2.0f * M_PI) * sample_rate_;
+
+    // Store the real pole frequency as the 'centre frequency' of the filterbank
+    // channel
+    output_.set_centre_frequency(i, frequency);
+
+    // From PZFC_Small_Signal_Params.m { From PZFC_Params.m {
+    float DpndF = ERBTools::Freq2ERB(frequency)
+                  / ERBTools::Freq2ERB(1000.0f) - 1.0f;
+
+    float p[8];  // Parameters (short name for ease of reading)
+
+    // Use parameter_values to recover the parameter values for this frequency
+    for (int param = 0; param < 7; ++param)
+      p[param] = parameter_values[param * 3]
+                 + parameter_values[param * 3 + 1] * DpndF
+                 + parameter_values[param * 3 + 2] * DpndF * DpndF;
+
+    // Calculate the final parameter
+    p[7] = p[1] * pow(10.0f, (p[2] / (p[1] * p[4])) * (p[6] - 60.0f) / 20.0f);
+    if (p[7] < 0.2f)
+      p[7] = 0.2f;
+
+    // Nominal bandwidth at this frequency
+    float fERBw = ERBTools::Freq2ERBw(frequency);
+
+    // Pole bandwidth
+    float fPBW = ((p[7] * fERBw * (2 * M_PI) / sample_rate_) / 2)
+                 * pow(p[4], 0.5f);
+
+    // Pole damping
+    float pole_damping = fPBW / sqrt(pow(pole_frequency, 2) + pow(fPBW, 2));
+
+    // Store the pole damping
+    pole_dampings_[i] = pole_damping;
+
+    // Zero bandwidth
+    float fZBW = ((p[0] * p[5] * fERBw * (2 * M_PI) / sample_rate_) / 2)
+                 * pow(p[4], 0.5f);
+
+    // Zero frequency
+    float zero_frequency = p[5] * pole_frequency;
+
+    if (zero_frequency > M_PI)
+      LOG_ERROR(_T("Warning: Zero frequency is above the Nyquist frequency "
+                   "in ModulePZFC(), continuing anyway but results may not "
+                   "be accurate."));
+
+    // Zero damping
+    float fZDamp = fZBW / sqrt(pow(zero_frequency, 2) + pow(fZBW, 2));
+
+    // Impulse-invariance mapping
+    float fZTheta = zero_frequency * sqrt(1.0f - pow(fZDamp, 2));
+    float fZRho = exp(-fZDamp * zero_frequency);
+
+    // Direct-form coefficients
+    float fA1 = -2.0f * fZRho * cos(fZTheta);
+    float fA2 = fZRho * fZRho;
+
+    // Normalised to unity gain at DC
+    float fASum = 1.0f + fA1 + fA2;
+    za0_[i] = 1.0f / fASum;
+    za1_[i] = fA1 / fASum;
+    za2_[i] = fA2 / fASum;
+
+    // Subtract step factor (1/n2) times current bandwidth from the pole
+    // frequency
+    pole_frequency -= ((1.0f / p[4])
+                       * (fERBw * (2.0f * M_PI) / sample_rate_));
+  }
+return true;
+}
+
+bool ModulePZFC::SetPZBankCoeffs() {
+  /*! \todo Re-implement the alternative parameter settings
+   */
+  if (!SetPZBankCoeffsERBFitted())
+    return false;
+
+  /*! \todo Make fMindamp and fMaxdamp user-settable?
+   */
+  mindamp_ = 0.18f;
+  maxdamp_ = 0.4f;
+
+  rmin_.resize(channel_count_);
+  rmax_.resize(channel_count_);
+  xmin_.resize(channel_count_);
+  xmax_.resize(channel_count_);
+
+  for (int c = 0; c < channel_count_; ++c) {
+    // Calculate maximum and minimum damping options
+    rmin_[c] = exp(-mindamp_ * pole_frequencies_[c]);
+    rmax_[c] = exp(-maxdamp_ * pole_frequencies_[c]);
+
+    xmin_[c] = rmin_[c] * cos(pole_frequencies_[c]
+                                      * pow((1-pow(mindamp_, 2)), 0.5f));
+    xmax_[c] = rmax_[c] * cos(pole_frequencies_[c]
+                                      * pow((1-pow(maxdamp_, 2)), 0.5f));
+  }
+
+  // Set up AGC parameters
+  agc_stage_count_ = 4;
+  agc_epsilons_.resize(agc_stage_count_);
+  agc_epsilons_[0] = 0.0064f;
+  agc_epsilons_[1] = 0.0016f;
+  agc_epsilons_[2] = 0.0004f;
+  agc_epsilons_[3] = 0.0001f;
+
+  agc_gains_.resize(agc_stage_count_);
+  agc_gains_[0] = 1.0f;
+  agc_gains_[1] = 1.4f;
+  agc_gains_[2] = 2.0f;
+  agc_gains_[3] = 2.8f;
+
+  float mean_agc_gain = 0.0f;
+  for (int c = 0; c < agc_stage_count_; ++c)
+    mean_agc_gain += agc_gains_[c];
+  mean_agc_gain /= static_cast<float>(agc_stage_count_);
+
+  for (int c = 0; c < agc_stage_count_; ++c)
+    agc_gains_[c] /= mean_agc_gain;
+
+  return true;
+}
+
+void ModulePZFC::AGCDampStep() {
+  if (detect_.size() == 0) {
+    // If  detect_ is not initialised, it means that the AGC is not set up.
+    // Set up now.
+    /*! \todo Make a separate InitAGC function which does this.
+     */
+    detect_.resize(channel_count_);
+    for (int c = 0; c < channel_count_; ++c)
+      detect_[c] = 1.0f;
+
+    float fDetectZero = DetectFun(0.0f);
+    for (int c = 0; c < channel_count_; c++)
+      detect_[c] *= fDetectZero;
+
+    for (int c = 0; c < channel_count_; c++)
+      for (int st = 0; st < agc_stage_count_; st++)
+        agc_state_[c][st] = (1.2f * detect_[c] * agc_gains_[st]);
+  }
+
+  float fAGCEpsLeft = 0.3f;
+  float fAGCEpsRight = 0.3f;
+
+  for (int c = channel_count_ - 1; c > -1; --c) {
+    for (int st = 0; st < agc_stage_count_; ++st) {
+      // This bounds checking is ugly and wasteful, and in an inner loop.
+      // If this algorithm is slow, this is why!
+      /*! \todo Proper non-ugly bounds checking in AGCDampStep()
+       */
+      float fPrevAGCState;
+      float fCurrAGCState;
+      float fNextAGCState;
+
+      if (c < channel_count_ - 1)
+        fPrevAGCState = agc_state_[c + 1][st];
+      else
+        fPrevAGCState = agc_state_[c][st];
+
+      fCurrAGCState = agc_state_[c][st];
+
+      if (c > 0)
+        fNextAGCState = agc_state_[c - 1][st];
+      else
+        fNextAGCState = agc_state_[c][st];
+
+      // Spatial smoothing
+      /*! \todo Something odd is going on here
+       *  I think this line is not quite right.
+       */
+      float agc_avg = fAGCEpsLeft * fPrevAGCState
+                      + (1.0f - fAGCEpsLeft - fAGCEpsRight) * fCurrAGCState
+                      + fAGCEpsRight * fNextAGCState;
+      // Temporal smoothing
+      agc_state_[c][st] = agc_avg * (1.0f - agc_epsilons_[st])
+                          + agc_epsilons_[st] * detect_[c] * agc_gains_[st];
+    }
+  }
+
+  float fOffset = 1.0f - agc_factor_ * DetectFun(0.0f);
+
+  for (int i = 0; i < channel_count_; ++i) {
+    float fAGCStateMean = 0.0f;
+    for (int j = 0; j < agc_stage_count_; ++j)
+     fAGCStateMean += agc_state_[i][j];
+
+    fAGCStateMean /= static_cast<float>(agc_stage_count_);
+
+    pole_damps_mod_[i] = pole_dampings_[i] *
+                           (fOffset + agc_factor_ * fAGCStateMean);
+  }
+}
+
+float ModulePZFC::DetectFun(float fIN) {
+  if (fIN < 0.0f)
+    fIN = 0.0f;
+  float fDetect = Minimum(1.0f, fIN);
+  float fA = 0.25f;
+  return fA * fIN + (1.0f - fA) * (fDetect - pow(fDetect, 3) / 3.0f);
+}
+
+inline float ModulePZFC::Minimum(float a, float b) {
+  if (a < b)
+    return a;
+  else
+    return b;
+}
+
+void ModulePZFC::Process(const SignalBank& input) {
+  // Set the start time of the output buffer
+  output_.set_start_time(input.start_time());
+
+  for (int iSample = 0; iSample < input.buffer_length(); ++iSample) {
+    float fInput = input[0][iSample];
+
+    // Lowpass filter the input with a zero at PI
+    fInput = 0.5f * fInput + 0.5f * last_input_;
+    last_input_ = input[0][iSample];
+
+    inputs_[channel_count_ - 1] = fInput;
+    for (int c = 0; c < channel_count_ - 1; ++c)
+      inputs_[c] = previous_out_[c + 1];
+
+    // PZBankStep2
+    // to save a bunch of divides
+    float damp_rate = 1.0f / (maxdamp_ - mindamp_);
+
+    for (int c = channel_count_ - 1; c > -1; --c) {
+      float interp_factor = (pole_damps_mod_[c]
+                             - mindamp_) * damp_rate;
+
+      float x = xmin_[c] + (xmax_[c] - xmin_[c]) * interp_factor;
+      float r = rmin_[c] + (rmax_[c] - rmin_[c]) * interp_factor;
+
+      // optional improvement to constellation adds a bit to r
+      float fd = pole_frequencies_[c] * pole_damps_mod_[c];
+      // quadratic for small values, then linear
+      r = r + 0.25f * fd * Minimum(0.05f, fd);
+
+      float zb1 = -2.0f * x;
+      float zb2 = r * r;
+
+      /* canonic poles but with input provided where unity DC gain is assured
+       * (mean value of state is always equal to mean value of input)
+       */
+      float new_state = inputs_[c] - (state_1_[c] - inputs_[c]) * zb1
+                                   - (state_2_[c] - inputs_[c]) * zb2;
+
+      // canonic zeros part as before:
+      float output = za0_[c] * new_state + za1_[c] * state_1_[c]
+                                         + za2_[c] * state_2_[c];
+
+      // cubic compression nonlinearity
+      output = output - 0.0001f * pow(output, 3);
+
+      output_.set_sample(c, iSample, output);
+      detect_[c] = DetectFun(output);
+      state_2_[c] = state_1_[c];
+      state_1_[c] = new_state;
+    }
+
+    if (do_agc_step_)
+      AGCDampStep();
+
+    for (int c = 0; c < channel_count_; ++c)
+      previous_out_[c] = output_[c][iSample];
+  }
+  PushOutput();
+}
+}  // namespace aimc