}//COMPUTE
private void coefficientBandPass(int order, double omega0, double omega1)
{
Complex[] poles = complexPolesBandPass(order, omega0, omega1);
poles = billinearTransformPoles(poles);
biQuadsSections = new BiQuad[order];
Parallel.For(0, poles.Length, k =>
{
biQuadsSections[k] = new BiQuad(0, -1, -2 * poles[k].re, Complex.AbsSqr(poles[k]));
});
if (order % 2 != 0)
{
double omega2 = Math.Sqrt(omega0 * omega1);
double p = -(omega1 - omega0) / omega2;
Complex pole;
if (Math.Abs(p) > 2)
{
double u = Math.Sqrt(-4 * Math.Pow(p, 2));
pole = 0.5 * omega2 * new Complex(p - u, 0);
}
else
{
double u = Math.Sqrt(4 - Math.Pow(p, 2));
pole = 0.5 * omega2 * new Complex(p - u, 0);
}
pole = billinearTransformPoles(pole);
biQuadsSections[poles.Length] = new BiQuad(0, -1, -2 * pole.re, Complex.AbsSqr(pole));
}
double omega = 2 * Math.Atan(omega0 * omega1);
biQuadsSections = normalizationBiQuadsSections(biQuadsSections, omega);
}
Complex BPSqrt(Complex root, int sign)
{
Complex a = root * root - 4;
double asq = Complex.AbsSqr(a);
double angle = Math.Atan2(a.im, a.re);
Complex sqrt = Math.Pow(asq, 0.25) * new Complex(Math.Cos(angle / 2), Math.Sin(angle / 2));
return(om_c * 0.5 * (root + sign * sqrt));
}
float[] Power(Complex[] H)
{
float[] power = new float[N];
for (int i = 0; i < N; i++)
{
power[i] = (float)Complex.AbsSqr(H[i]);
}
return(power);
}
private void NormalizeBiQuads(BiQuad[] biQuads, double omegaRef)
{
Complex z_1 = new Complex(Math.Cos(omegaRef), -Math.Sin(omegaRef));
double scale;
for (int i = 0; i < biQuads.Length; i++)
{
Complex numerator = (biQuads[i].b0 + biQuads[i].b1 * z_1 + biQuads[i].b2 * z_1 * z_1);
Complex denominator = (biQuads[i].a0 + biQuads[i].a1 * z_1 + biQuads[i].a2 * z_1 * z_1);
scale = 1 / Math.Sqrt(Complex.AbsSqr(numerator / denominator));
biQuads[i].b0 *= scale;
biQuads[i].b1 *= scale;
biQuads[i].b2 *= scale;
}
}
double DGain(double gain, Complex omref, Complex[] zeros, Complex[] poles)
{
if (zeros != null)
{
for (int i = 0; i < zeros.Length; i++)
{
gain /= Math.Sqrt(Complex.AbsSqr(omref - zeros[i]));
}
}
for (int i = 0; i < poles.Length; i++)
{
gain *= Math.Sqrt(Complex.AbsSqr(omref - poles[i]));
}
return(gain);
}
public double FrequencyResponse(double f, double fs)
{
double omega = 2 * Math.PI * f / fs;
Complex Z_1 = new Complex(Math.Cos(omega), -Math.Sin(omega));
double frequencyResponse = 1;
for (int i = 0; i < biQuads.Length; i++)
{
Complex numerator = (biQuads[i].b0 + biQuads[i].b1 * Z_1 + biQuads[i].b2 * Z_1 * Z_1);
Complex denominator = (biQuads[i].a0 + biQuads[i].a1 * Z_1 + biQuads[i].a2 * Z_1 * Z_1);
frequencyResponse *= Math.Sqrt(Complex.AbsSqr(numerator / denominator));
}
return(frequencyResponse);
}
private BiQuad[] normalizationBiQuadsSections(BiQuad[] biQuadsSections, double omega)
{
Complex z1 = new Complex(Math.Cos(omega), -Math.Sin(omega));
double skala;
for (int k = 0; k < biQuadsSections.Length; k++)
{
Complex num = biQuadsSections[k].b0 + biQuadsSections[k].b1 * z1 * biQuadsSections[k].b2 * z1 * z1;
Complex denom = biQuadsSections[k].a0 + biQuadsSections[k].a1 * z1 + biQuadsSections[k].a2 * z1 * z1;
skala = 1 / Math.Sqrt(Complex.AbsSqr(num / denom));
biQuadsSections[k].b0 *= skala;
biQuadsSections[k].b1 *= skala;
biQuadsSections[k].b2 *= skala;
}
return(biQuadsSections);
}
private void coefficientLowPass(int order, double omega)
{
Complex[] poles = complexPolesLowPass(order, omega);
poles = billinearTransformPoles(poles);
biQuadsSections = new BiQuad[(order + 1) / 2];
Parallel.For(0, poles.Length, k =>
{
biQuadsSections[k] = new BiQuad(0, -1, -2 * poles[k].re, Complex.AbsSqr(poles[k]));
});
if (order % 2 != 0)
{
double pole = billinearTransformOmega(omega);
biQuadsSections[poles.Length] = new BiQuad(1, 0, -pole, 0);
}
biQuadsSections = normalizationBiQuadsSections(biQuadsSections, omega);
}
private void CalculateFilterCoefficientsLP(int order, double omega0)
{
Complex[] poles = GetComplexPolesLP(order, omega0);
BilinearTransform(poles);
biQuads = new BiQuad[(order + 1) / 2];
for (int i = 0; i < poles.Length; i++)
{
biQuads[i] = new BiQuad(2, 1, -2 * poles[i].re, Complex.AbsSqr(poles[i]));
}
if (order % 2 != 0)
{
double pole = BilinearTransform(-omega0);
biQuads[poles.Length] = new BiQuad(1, 0, -pole, 0);
}
NormalizeBiQuads(biQuads, 0);
}
private void CalculateFilterCoefficientsBP(int order, double omega0, double omega1)
{
Complex[] poles = GetComplexPolesBP(order, omega0, omega1);
BilinearTransform(poles);
biQuads = new BiQuad[order];
for (int i = 0; i < poles.Length; i++)
{
biQuads[i] = new BiQuad(0, -1, -2 * poles[i].re, Complex.AbsSqr(poles[i]));
}
if (order % 2 != 0)
{
double omega2 = Math.Sqrt(omega0 * omega1);
double p = -(omega1 - omega0) / omega2;
Complex pole;
if (Math.Abs(p) > 2)
{
double u = Math.Sqrt(-4 + Math.Pow(p, 2));
pole = 0.5 * omega2 * new Complex(p - u, 0);
}
else
{
double u = Math.Sqrt(4 - Math.Pow(p, 2));
pole = 0.5 * omega2 * new Complex(p, u);
}
pole = BilinearTransform(pole);
biQuads[poles.Length] = new BiQuad(0, -1, -2 * pole.re, Complex.AbsSqr(pole));
}
double omegaRef = 2 * Math.Atan(Math.Sqrt(omega0 * omega1));
NormalizeBiQuads(biQuads, omegaRef);
}
public void Calculate(double[] input)
{
int count = (int)pCount.data;
if (averagingType == AveragingType.Linear || count < numberOfAverages)
{
alpha = (double)count / (count + 1);
}
else
{
alpha = 1 - 2.0 / (numberOfAverages + 1);
}
beta = 1 - alpha;
for (int i = 0; i < input.Length; i++)
{
timeSignal[i] = input[i];
temp[i].re = input[i] * hanning[i];
temp[i].im = 0;
}
fft.FftForward(temp);
for (int i = 0; i < temp.Length; i++)
{
instSpectrum[i] = temp[i];
}
for (int i = 0; i < autoSpectrumLength; i++)
{
double power = Complex.AbsSqr(temp[i]) * factor;
if (i == 0)
{
power *= 0.5;
}
autoSpectrum[i] = autoSpectrum[i] * alpha + power * beta;
}
int N = temp.Length;
for (int i = 0; i < N; i++)
{
temp[i] = new Complex(0, 0);
}
for (int i = 0; i < autoSpectrum.Length; i++)
{
temp[i].re = Math.Sqrt(autoSpectrum[i]);
}
double min = double.MaxValue;
for (int i = 0; i <= N / 2; i++)
{
if (temp[i].re > 0 && temp[i].re < min)
{
min = temp[i].re;
}
}
for (int i = 0; i <= N / 2; i++)
{
if (temp[i].re == 0)
{
temp[i].re = min;
}
}
for (int i = 0; i <= N / 2; i++)
{
temp[i].re = 2 * Math.Log(temp[i].re);
}
//int n0 = (int)(20.0 / 51200 * N);
//for (int i = 0; i <= n0; i++)
// temp[i].re *= 0.5 * (1 - Math.Cos(Math.PI / n0 * i));
//int n2 = (int)(10000.0 / 51200 * N);
//for (int i = n2; i <= N / 2; i++)
// temp[i].re *= 0.5 * (1 + Math.Cos(Math.PI / (N / 2 - n2) * (i - n2)));
for (int i = 1; i < N / 2; i++)
{
temp[N - i].re = temp[i].re;
}
fft.FftInverse(temp);
int n3 = N / 2 + 1;
int n4 = N / 2 + 1;
for (int i = n3; i < n4; i++)
{
temp[i] *= 0.5 * (1 + Math.Cos(Math.PI / (n4 - n3) * (i - n3)));
}
for (int i = n4; i < N; i++)
{
temp[i] = new Complex(0, 0);
}
n4 = 1;
for (int i = 0; i < n4; i++)
{
double fact = 0.5 * (1 + Math.Sin(Math.PI / 2 / n4 * i));
temp[i] *= fact;
temp[N / 2 - i] *= fact;
}
fft.FftForward(temp);
for (int i = 0; i < N / 2; i++)
{
temp[i] = Math.Exp(temp[i].re) * new Complex(Math.Cos(temp[i].im), Math.Sin(temp[i].im));
temp[i + N / 2] = new Complex(0, 0);
}
//for (int i = 0; i <= n0; i++)
// temp[i] = new Complex(0, 0);
//for (int i = n2; i < N; i++)
// temp[i] = new Complex(0, 0);
fft.FftInverse(temp);
int skip = (int)(0.005 * 51200);
double max = double.MinValue;
for (int i = skip; i < N; i++)
{
if (Math.Abs(temp[i].re) > max)
{
max = Math.Abs(temp[i].re);
}
}
double factor1 = 30000 / max;
for (int i = skip; i < N; i++)
{
temp[i - skip] = temp[i] * factor1;
}
for (int i = N - skip; i < N; i++)
{
temp[i] = new Complex(0, 0);
}
for (int i = 0; i < N / 2; i++)
{
autocorrelation[i] = (autocorrelation[i] * count + temp[i].re) / (count + 1);
}
count++;
if (false)
{
WaveIO waveIO = new WaveIO();
waveIO.SamplingFrequency = 51200;
waveIO.SaveRealData("c:/users/jhee/impavg.wav", autocorrelation, 1);
}
pCount.data = count;
}
