double filter(BiquadLPF bq, double input)
{
double w = input - bq.a[0] * bq.hist[0] - bq.a[1] * bq.hist[1];
double output = bq.b[1] * bq.hist[0] + bq.b[2] * bq.hist[1] + bq.b[0] * w;
bq.hist[1] = bq.hist[0];
bq.hist[0] = w;
return(output);
}
public BiquadFilter(int samplingRate, int frameSize)
{
this.samplingRate = samplingRate;
this.frameSize = frameSize;
driftFilter = createLPF(1.0 / 180.0, 0.3);
errorFilter = createLPF(1.0 / 10.0, 0.25);
errorDerivFilter = createLPF(1.0 / 2.0, 0.2);
playbackRate = 1.0;
estError = 0;
lastError = 0;
desiredFill = 0;
fillCount = 0;
initialFill = START_OFFSET + INITIAL_SAMPLES;
}
BiquadLPF createLPF(double freq, double Q)
{
BiquadLPF ret = new BiquadLPF();
double w0 = 2 * Math.PI * freq * frameSize / (double)samplingRate;
double alpha = Math.Sin(w0) / (2.0 * Q);
double a_0 = 1.0 + alpha;
ret.b[0] = (1.0 - Math.Cos(w0)) / (2.0 * a_0);
ret.b[1] = (1.0 - Math.Cos(w0)) / a_0;
ret.b[2] = ret.b[0];
ret.a[0] = -2.0 * Math.Cos(w0) / a_0;
ret.a[1] = (1 - alpha) / a_0;
return(ret);
}