БИХ-фильтры 2-го порядка

Код

/**

* БИХ-фильтры 2-го порядка

* расчет взят с http://model.exponenta.ru/audio_eq.html

*/

public class IIR {

 public static final byte OFF       = 0;

 public static final byte LOWPASS = 1;

 public static final byte HIGHPASS  = 2;

 public static final byte RESONANCE = 3;

 public static final byte BANDPASS1 = 4;

 public static final byte BANDPASS2 = 5;

 public static final byte REJECT   = 6;

 public static final byte LOWRES    = 8;

 public static final byte HIGHRES   = 7;

 //

 private int m_sampleRate;

 private byte m_type;

 private int m_frequency;

 private double m_Q;

 private int m_dBgain;

 //

 private double B0, B1, B2, A1, A2, x2, x1, y2, y1;

 //

 public IIR(int sampleRate)

 {

         m_sampleRate = sampleRate;

         reset();

 }

 public void reset() {// выключение фильтра и обнуление истории

         B0 = 1.0;

         B1 = B2 = A1 = A2 = x2 = x1 = y2 = y1 = 0.0;

         m_type = IIR.OFF;

         m_frequency = -1;

         m_Q = -1;

         m_dBgain = 0;

 }

 public byte getType() { return m_type; }

 public int getFrequency() { return m_frequency; }

 public double getQ() { return m_Q; }

 public int getGain() { return m_dBgain; }

 public IIR getClone() {// клонирование фильтра вместе с состоянием

         IIR flt = new IIR( m_sampleRate );

         flt.m_type = this.m_type;

         flt.m_frequency = this.m_frequency;

         flt.m_Q = this.m_Q;

         flt.m_dBgain = this.m_dBgain;

         //

         flt.B0 = this.B0;

         flt.B1 = this.B1;

         flt.B2 = this.B2;

         flt.A1 = this.A1;

         flt.A2 = this.A2;

         flt.x2 = this.x2;

         flt.x1 = this.x1;

         flt.y2 = this.y2;

         flt.y1 = this.y1;

         return flt;

 }

 public static void copy(IIR src, IIR dst)

 {// копирование параметров фильтра вместе с состоянием

         dst.m_sampleRate = src.m_sampleRate;

         dst.m_type = src.m_type;

         dst.m_frequency = src.m_frequency;

         dst.m_Q = src.m_Q;

         dst.m_dBgain = src.m_dBgain;

         //

         dst.B0 = src.B0;

         dst.B1 = src.B1;

         dst.B2 = src.B2;

         dst.A1 = src.A1;

         dst.A2 = src.A2;

         dst.x2 = src.x2;

         dst.x1 = src.x1;

         dst.y2 = src.y2;

         dst.y1 = src.y1;

 }

 public static void copyState(IIR src, IIR dst) {

         dst.x2 = src.x2;

         dst.x1 = src.x1;

         dst.y2 = src.y2;

         dst.y1 = src.y1;

 }

 public void reinit(byte type, int frequency, double Q, int dBgain)

 {// перестройка фильтра, -1 (-128 для dBgain) означает, что параметр не меняется

         init( type < 0 ? m_type : type,

                 frequency < 0 ? m_frequency : frequency,

                 Q < 0 ? m_Q : Q,

                 dBgain < -127 ? m_dBgain : dBgain );

 }

 public void init(byte type, int frequency, double Q, int dBgain)

 {// инициализация, расчет параметров

         m_type = type;

         m_frequency = frequency;

         m_Q = Q;

         m_dBgain = dBgain;

         double omega = 2.0 * Math.PI * (double)frequency / (double)m_sampleRate;

         double cos = Math.cos( omega );

         double A = Math.sqrt( Math.pow( 10.0, dBgain / 20.0 ) );

         double alpha = Math.sin( omega ) / ( 2.0 * Q );

         //если задана полоса, то другая формула:

    //alpha = Math.sin( omega ) * Math.sinh( ln(2)/2 * bandWidth * omega / Math.sin( omega ) );

         double beta  = Math.sqrt( A ) / Q;

         //если задан наклон горки, то другая формула:

    //beta  = sqrt(A)*sqrt[ (A + 1/A)*(1/S - 1) + 2 ]

    //beta  = sqrt[ (A^2 + 1)/S - (A-1)^2 ]

         double A0 = 1.0 + alpha;

         switch( type ) {

         default:

         case OFF:

                 B0 = 1.0;

                 B1 = B2 = A1 = A2 = 0.0;

                 break;

         case LOWPASS:

                 B1 = ( 1.0 - cos ) / A0;

                 B0 = B1 / 2.0;

                 B2 = B0;

                 A1 = -2.0 * cos / A0;

                 A2 = ( 1 - alpha ) / A0;

                 break;

         case HIGHPASS:

                 B1 = -( 1.0 + cos ) / A0;

                 B0 = -B1 / 2.0;

                 B2 = B0;

                 A1 = -2.0 * cos / A0;

                 A2 = ( 1.0 - alpha ) / A0;

                 break;

         case RESONANCE:

                 A0 = 1.0 + alpha / A;

                 B0 = ( 1.0 + alpha * A ) / A0;

                 B1 = -2.0 * cos / A0;

                 B2 = ( 1.0 - alpha * A ) / A0;

                 A1 = B1;

                 A2 = ( 1.0 - alpha / A ) / A0;

                 break;

         case BANDPASS1://max A = Q

                 B0 = Q * alpha / A0;

                 B1 = 0.0;

                 B2 = -B0;

                 A1 = -2.0 * cos / A0;

                 A2 = ( 1.0 - alpha ) / A0;

                 break;

         case BANDPASS2:// max A = 0 dB

                 B0 = alpha / A0;

                 B1 = 0.0;

                 B2 = -B0;

                 A1 = -2.0 * cos / A0;

                 A2 = ( 1.0 - alpha ) / A0;

                 break;

         case REJECT:

                 B0 = 1.0 / A0;

                 B1 = -2 * cos / A0;

                 B2 = B0;

                 A1 = B1;

                 A2 = ( 1 - alpha ) / A0;

                 break;

         case LOWRES: {

                 double ap = A + 1.0;

                 double am = A - 1.0;

                 double amcos = am * cos;

                 double apcos = ap * cos;

                 double bsin = beta * Math.sin( omega );

                 A0 =             ap + amcos + bsin;

                 B0 =       A * ( ap - amcos + bsin ) / A0;

                 B1 = 2.0 * A * ( am - apcos ) / A0;

                 B2 =       A * ( ap - amcos - bsin ) / A0;

                 A1 =    -2.0 * ( am + apcos ) / A0;

                 A2 =           ( ap + amcos - bsin ) / A0;

                 break;

         }

         case HIGHRES: {

                 double ap = A + 1.0;

                 double am = A - 1.0;

                 double amcos = am * cos;

                 double apcos = ap * cos;

                 double bsin = beta * Math.sin( omega );

                 A0 =              ap - amcos + bsin;

                 B0 =        A * ( ap + amcos + bsin ) / A0;

                 B1 = -2.0 * A * ( am + apcos ) / A0;

                 B2 =        A * ( ap + amcos - bsin ) / A0;

                 A1 =      2.0 * ( am - apcos ) / A0;

                 A2 =            ( ap - amcos - bsin ) / A0;

                 break;

         }

         }

 }

 public double proc(double sample)

 {// расчет отклика для каскадного включения без усечения точности

         double y = B0 * sample + B1 * x1 + B2 * x2 - A1 * y1 - A2 * y2;

         x2 = x1;

         x1 = sample;

         y2 = y1;

         y1 = y;

         return y;

 }

 public int proc(int sample)

 {// расчет отклика

         double y = proc( (double)sample );

         return (int)(y > 0 ? y + 0.5 : y - 0.5);

 }

}