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