/// <summary>
/// dspguide.com Chapter 20
/// </summary>
/// <param name="frequencyCutoff">0 to .5 relative to sampling frequency</param>
/// <param name="highPass">false for lowpass, true for highpass</param>
/// <param name="percentRipple">0 to 29 percent ripple, .5 recommended</param>
/// <param name="numPoles">Number of poles, must be even</param>
///
public static FilterCoefs generateCoefficients(double frequencyCutoff, bool highPass, double percentRipple, int numPoles)
{
double[] a = new double[22];
double[] b = new double[22];
double[] ta = new double[22];
double[] tb = new double[22];
double a0, a1, a2, b1, b2;
for (int i = 0; i < 22; i++)
{
a[i] = 0; b[i] = 0;
}
a[2] = 1;
b[2] = 1;
double k = 0;
for (int p = 1; p <= (numPoles / 2); p++)
{
var rp = -Math.Cos(Math.PI / (numPoles * 2) + (p - 1) * Math.PI / numPoles);
var ip = Math.Sin(Math.PI / (numPoles * 2) + (p - 1) * Math.PI / numPoles);
if (percentRipple != 0)
{ //Warp from a circle to an ellipse
var es = Math.Sqrt(Math.Pow(100 / (100 - percentRipple), 2) - 1);
var vx = (1.0 / numPoles) * Math.Log((1.0 / es) + Math.Sqrt((1.0 / Math.Pow(es, 2)) + 1));
var kx = (1.0 / numPoles) * Math.Log((1.0 / es) + Math.Sqrt((1.0 / Math.Pow(es, 2)) - 1));
kx = (Math.Exp(kx) + Math.Exp(-kx)) / 2;
rp = rp * ((Math.Exp(vx) - Math.Exp(-vx)) / 2) / kx;
ip = ip * ((Math.Exp(vx) + Math.Exp(-vx)) / 2) / kx;
}
//s-domain to z-domain conversion
var t = 2 * Math.Tan(.5);
var w = 2 * Math.PI * frequencyCutoff;
var m = Math.Pow(rp, 2) + Math.Pow(ip, 2);
var d = 4 - 4 * rp * t + m * Math.Pow(t, 2);
var x0 = Math.Pow(t, 2) / d;
var x1 = 2 * Math.Pow(t, 2) / d;
var x2 = Math.Pow(t, 2) / d;
var y1 = (8 - 2 * m * Math.Pow(t, 2)) / d;
var y2 = (-4 - 4 * rp * t - m * Math.Pow(t, 2)) / d;
//lp to lp or lp to hp transform
if (highPass)
{
k = -Math.Cos(w / 2 + .5) / Math.Cos(w / 2 - .5);
}
if (!highPass)
{
k = Math.Sin(.5 - w / 2) / Math.Sin(.5 + w / 2);
}
d = 1 + y1 * k - y2 * Math.Pow(k, 2);
a0 = (x0 - x1 * k + x2 * Math.Pow(k, 2)) / d;
a1 = (-2 * x0 * k + x1 + x1 * Math.Pow(k, 2) - 2 * x2 * k) / d;
a2 = (x0 * Math.Pow(k, 2) - x1 * k + x2) / d;
b1 = (2 * k + y1 + y1 * Math.Pow(k, 2) - 2 * y2 * k) / d;
b2 = (-(Math.Pow(k, 2)) - y1 * k + y2) / d;
if (highPass)
{
a1 = -a1;
}
if (highPass)
{
b1 = -b1;
}
for (int i = 0; i < 22; i++)
{
ta[i] = a[i];
tb[i] = b[i];
}
for (int i = 2; i < 22; i++)
{
a[i] = a0 * ta[i] + a1 * ta[i - 1] + a2 * ta[i - 2];
b[i] = tb[i] - b1 * tb[i - 1] - b2 * tb[i - 2];
}
}
b[2] = 0;
for (int i = 0; i < 20; i++)
{
a[i] = a[i + 2];
b[i] = -b[i + 2];
}
double sa = 0, sb = 0;
for (int i = 0; i < 20; i++)
{
if (!highPass)
{
sa += a[i];
}
if (!highPass)
{
sb += b[i];
}
if (highPass)
{
sa += a[i] * Math.Pow(-1, i);
}
if (highPass)
{
sb += b[i] * Math.Pow(-1, i);
}
}
double gain = sa / (1 - sb);
for (int i = 0; i < 20; i++)
{
a[i] = a[i] / gain;
}
FilterCoefs ret = new FilterCoefs();
ret.A = new double[numPoles + 1];
ret.B = new double[numPoles];
for (int i = 0; i <= numPoles; i++)
{
ret.A[i] = a[i];
}
for (int i = 1; i <= numPoles; i++)
{
ret.B[i - 1] = b[i];
}
return(ret);
}
public static double[] LowPass(double freq, int sampleRate, double[] data)
{
double fc = (double)freq / sampleRate;
int numOfPoles = 0;
if (fc <= .02)
{
numOfPoles = 4;
}
else if (fc <= .05)
{
numOfPoles = 6;
}
else if (fc <= .10)
{
numOfPoles = 10;
}
else if (fc <= .25)
{
numOfPoles = 18;
}
else if (fc <= .40)
{
numOfPoles = 10;
}
else if (fc <= .45)
{
numOfPoles = 6;
}
else
{
numOfPoles = 4;
}
FilterCoefs coefs = generateCoefficients(fc, false, 2, numOfPoles);
double[] paddedData = new double[data.Length + 100];
double[] newData = new double[data.Length + 100];
for (int i = 0; i < 50; i++)
{
paddedData[i] = 0;
paddedData[paddedData.Length - i - 1] = 0;
}
for (int i = 0; i < data.Length; i++)
{
paddedData[i + 50] = data[i];
}
for (int i = 20; i < (paddedData.Length - numOfPoles); i++)
{
var current = paddedData[i] * coefs.A[0];
double future = 0;
double past = 0;
for (int x = 1; x < coefs.A.Length; x++)
{
future += paddedData[i + x] * coefs.A[x];
past += newData[i - x] * coefs.B[x - 1];
}
newData[i] = current + future + past;
}
double[] ret = new double[data.Length];
for (int i = 0; i < data.Length; i++)
{
ret[i] = newData[50 + i];
if (ret[i] > 1)
{
ret[i] = 1;
}
if (ret[i] < -1)
{
ret[i] = -1;
}
}
return(ret);
}
/// <summary>
/// dspguide.com Chapter 20
/// </summary>
/// <param name="frequencyCutoff">0 to .5 relative to sampling frequency</param>
/// <param name="highPass">false for lowpass, true for highpass</param>
/// <param name="percentRipple">0 to 29 percent ripple, .5 recommended</param>
/// <param name="numPoles">Number of poles, must be even</param>
///
public static FilterCoefs generateCoefficients(double frequencyCutoff, bool highPass, double percentRipple, int numPoles)
{
double[] a = new double[22];
double[] b = new double[22];
double[] ta = new double[22];
double[] tb = new double[22];
double a0, a1, a2, b1, b2;
for (int i = 0; i < 22; i++) { a[i] = 0; b[i] = 0; }
a[2] = 1;
b[2] = 1;
double k = 0;
for (int p = 1; p <= (numPoles / 2); p++)
{
var rp = -Math.Cos(Math.PI / (numPoles * 2) + (p - 1) * Math.PI / numPoles);
var ip = Math.Sin(Math.PI / (numPoles * 2) + (p - 1) * Math.PI / numPoles);
if (percentRipple != 0)
{ //Warp from a circle to an ellipse
var es = Math.Sqrt(Math.Pow(100 / (100 - percentRipple), 2) - 1);
var vx = (1.0 / numPoles) * Math.Log((1.0 / es) + Math.Sqrt((1.0 / Math.Pow(es, 2)) + 1));
var kx = (1.0 / numPoles) * Math.Log((1.0 / es) + Math.Sqrt((1.0 / Math.Pow(es, 2)) - 1));
kx = (Math.Exp(kx) + Math.Exp(-kx)) / 2;
rp = rp * ((Math.Exp(vx) - Math.Exp(-vx)) / 2) / kx;
ip = ip * ((Math.Exp(vx) + Math.Exp(-vx)) / 2) / kx;
}
//s-domain to z-domain conversion
var t = 2 * Math.Tan(.5);
var w = 2 * Math.PI * frequencyCutoff;
var m = Math.Pow(rp, 2) + Math.Pow(ip, 2);
var d = 4 - 4 * rp * t + m * Math.Pow(t, 2);
var x0 = Math.Pow(t, 2) / d;
var x1 = 2 * Math.Pow(t, 2) / d;
var x2 = Math.Pow(t, 2) / d;
var y1 = (8 - 2 * m * Math.Pow(t, 2)) / d;
var y2 = (-4 - 4 * rp * t - m * Math.Pow(t, 2)) / d;
//lp to lp or lp to hp transform
if (highPass) k = -Math.Cos(w / 2 + .5) / Math.Cos(w / 2 - .5);
if (!highPass) k = Math.Sin(.5 - w / 2) / Math.Sin(.5 + w / 2);
d = 1 + y1 * k - y2 * Math.Pow(k, 2);
a0 = (x0 - x1 * k + x2 * Math.Pow(k, 2)) / d;
a1 = (-2 * x0 * k + x1 + x1 * Math.Pow(k, 2) - 2 * x2 * k) / d;
a2 = (x0 * Math.Pow(k, 2) - x1 * k + x2) / d;
b1 = (2 * k + y1 + y1 * Math.Pow(k, 2) - 2 * y2 * k) / d;
b2 = (-(Math.Pow(k, 2)) - y1 * k + y2) / d;
if (highPass) a1 = -a1;
if (highPass) b1 = -b1;
for (int i = 0; i < 22; i++)
{
ta[i] = a[i];
tb[i] = b[i];
}
for (int i = 2; i < 22; i++)
{
a[i] = a0 * ta[i] + a1 * ta[i - 1] + a2 * ta[i - 2];
b[i] = tb[i] - b1 * tb[i - 1] - b2 * tb[i - 2];
}
}
b[2] = 0;
for (int i = 0; i < 20; i++)
{
a[i] = a[i + 2];
b[i] = -b[i + 2];
}
double sa = 0, sb = 0;
for (int i = 0; i < 20; i++)
{
if (!highPass) sa += a[i];
if (!highPass) sb += b[i];
if (highPass) sa += a[i] * Math.Pow(-1, i);
if (highPass) sb += b[i] * Math.Pow(-1, i);
}
double gain = sa / (1 - sb);
for (int i = 0; i < 20; i++)
a[i] = a[i] / gain;
FilterCoefs ret = new FilterCoefs();
ret.A = new double[numPoles + 1];
ret.B = new double[numPoles];
for (int i = 0; i <= numPoles; i++)
ret.A[i] = a[i];
for (int i = 1; i <= numPoles; i++)
ret.B[i - 1] = b[i];
return ret;
}
