init
This commit is contained in:
@@ -0,0 +1,214 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace Exocortex.DSP
|
||||
{
|
||||
public enum PassFilterType
|
||||
{
|
||||
Bessel,
|
||||
Butterworth,
|
||||
Chebyshev,
|
||||
CriticalDamping
|
||||
}
|
||||
public static class PassFilter
|
||||
{
|
||||
public static double[] HighPass(double[] values, double sampleRate, double centerFrequency, PassFilterType type, uint order)
|
||||
{
|
||||
return RunFilter(values, sampleRate, centerFrequency, type, order, false);
|
||||
}
|
||||
public static double[] LowPass(double[] values, double sampleRate, double centerFrequency, PassFilterType type, uint order)
|
||||
{
|
||||
return RunFilter(values, sampleRate, centerFrequency, type, order, true);
|
||||
}
|
||||
private static double[] RunFilter(double[] values, double sampleRate, double centerFrequency, PassFilterType type, uint order, bool lowPass)
|
||||
{
|
||||
Complex[] data = new Complex[values.Length];
|
||||
for (int i = 0; i < values.Length; i++)
|
||||
{
|
||||
data[i] = (Complex)values[i];
|
||||
}
|
||||
Complex[] result;
|
||||
switch (type)
|
||||
{
|
||||
case PassFilterType.Bessel:
|
||||
result = BesselFilter(data, sampleRate, centerFrequency, order, lowPass);
|
||||
break;
|
||||
case PassFilterType.Chebyshev:
|
||||
result = ChebyshevFilter(data, sampleRate, centerFrequency, order, 1D, 0.005, lowPass);
|
||||
break;
|
||||
case PassFilterType.Butterworth:
|
||||
default:
|
||||
result = ButterworthFilter(data, sampleRate, centerFrequency, order, 1D, lowPass);
|
||||
break;
|
||||
}
|
||||
return result.Select(c => c.Re).ToArray();
|
||||
}
|
||||
private static Complex[] BesselFilter(Complex[] values, double sampleRate, double centerFrequency, uint order, bool lowPass)
|
||||
{
|
||||
var signal = (Complex[])values.Clone();
|
||||
var N = signal.Length;
|
||||
|
||||
// Get the reverse Bessel polynomial-to-gain function denominator
|
||||
Polynomial B = BesselDenominatorPolynomial((int)order);
|
||||
var numerator = Math.Sqrt(B.GetCoefficient(0));
|
||||
// Apply forward FFT
|
||||
Fourier.FFT(signal, N, FourierDirection.Forward);
|
||||
// Apply gain function
|
||||
if (centerFrequency > 0)
|
||||
{
|
||||
var numBins = N / 2; // Half the length of the FFT by symmetry
|
||||
var binWidth = sampleRate / N; // Hz
|
||||
|
||||
// Filter
|
||||
System.Threading.Tasks.Parallel.For(1, N / 2, i =>
|
||||
{
|
||||
var binFreq = binWidth * i;
|
||||
var gain = BesselGain(numerator, B, binFreq, centerFrequency, lowPass);
|
||||
signal[i] *= gain;
|
||||
signal[N - i] *= gain;
|
||||
});
|
||||
|
||||
}
|
||||
// Reverse filtered signal
|
||||
Fourier.FFT(signal, N, FourierDirection.Backward);
|
||||
signal = signal.Select(n => n / N).ToArray(); // scale result by length
|
||||
|
||||
return signal;
|
||||
}
|
||||
private static double BesselGain(double numerator, Polynomial B, double freq, double centerFreq, bool lowPass)
|
||||
{
|
||||
return numerator / Math.Sqrt(B.Evaluate(lowPass ? freq / centerFreq : centerFreq / freq)); // low-to-high pass is inverting f/fc
|
||||
}
|
||||
|
||||
// uses logic from https://www.centerspace.net/butterworth-filter-csharp
|
||||
private static Complex[] ButterworthFilter(Complex[] values, double sampleRate, double centerFrequency, uint order, double DCGain, bool lowPass)
|
||||
{
|
||||
var signal = (Complex[])values.Clone();
|
||||
var N = signal.Length;
|
||||
|
||||
// Apply forward FFT
|
||||
Fourier.FFT(signal, N, FourierDirection.Forward);
|
||||
// Apply gain function
|
||||
if (centerFrequency > 0)
|
||||
{
|
||||
var numBins = N / 2; // Half the length of the FFT by symmetry
|
||||
var binWidth = sampleRate / N; // Hz
|
||||
|
||||
// Filter
|
||||
System.Threading.Tasks.Parallel.For(1, N / 2, i =>
|
||||
{
|
||||
var binFreq = binWidth * i;
|
||||
var gain = ButterworthGain(DCGain, binFreq, centerFrequency, order, lowPass);
|
||||
signal[i] *= gain;
|
||||
signal[N - i] *= gain;
|
||||
});
|
||||
|
||||
}
|
||||
// Reverse filtered signal
|
||||
Fourier.FFT(signal, N, FourierDirection.Backward);
|
||||
// scale result by length
|
||||
signal = signal.Select(n => n / N).ToArray();
|
||||
return signal;
|
||||
}
|
||||
private static double ButterworthGain(double DCGain, double freq, double centerFreq, uint order, bool lowPass)
|
||||
{
|
||||
return DCGain / Math.Sqrt(1 + Math.Pow(lowPass ? freq / centerFreq : centerFreq / freq, 2.0 * order)); // low-to-high pass is just inverting f/fc
|
||||
}
|
||||
|
||||
// uses logic from https://www.centerspace.net/chebyshev-filter-csharp
|
||||
private static Complex[] ChebyshevFilter(Complex[] values, double sampleRate, double centerFrequency, uint order, double DCGain, double ripple, bool lowPass)
|
||||
{
|
||||
var signal = (Complex[])values.Clone();
|
||||
var N = signal.Length;
|
||||
|
||||
// Get the Chebyshev polynomial
|
||||
Polynomial T = ChebyshevPolynomial((int)order);
|
||||
// Apply forward FFT
|
||||
Fourier.FFT(signal, N, FourierDirection.Forward);
|
||||
// Remove DC offset
|
||||
signal[0] = (Complex)0;
|
||||
|
||||
// Apply gain function
|
||||
if (centerFrequency > 0)
|
||||
{
|
||||
var numBins = N / 2; // Half the length of the FFT by symmetry
|
||||
var binWidth = sampleRate / N; // Hz
|
||||
|
||||
// Filter
|
||||
System.Threading.Tasks.Parallel.For(1, N / 2, i =>
|
||||
{
|
||||
var binFreq = binWidth * i;
|
||||
var gain = ChebyshevGain(DCGain, ripple, T, binFreq, centerFrequency, order, lowPass);
|
||||
signal[i] *= gain;
|
||||
signal[N - i] *= gain;
|
||||
});
|
||||
|
||||
}
|
||||
// Reverse filtered signal
|
||||
Fourier.FFT(signal, N, FourierDirection.Backward);
|
||||
// scale result by length
|
||||
signal = signal.Select(n => n / N).ToArray();
|
||||
|
||||
return signal;
|
||||
}
|
||||
private static double ChebyshevGain(double DCGain, double ripple, Polynomial T, double freq, double centerFreq, uint order, bool lowPass)
|
||||
{
|
||||
return DCGain / Math.Sqrt(1 + ripple * Math.Pow(T.Evaluate(lowPass ? freq / centerFreq : centerFreq / freq), 2.0 * order));
|
||||
}
|
||||
|
||||
private static Polynomial ChebyshevPolynomial(int order)
|
||||
{
|
||||
switch (order)
|
||||
{
|
||||
case 0:
|
||||
return new SimplePolynomial(new double[] { 1 });
|
||||
case 1:
|
||||
return new SimplePolynomial(new double[] { 0, 1 });
|
||||
case 2:
|
||||
return new SimplePolynomial(new double[] { -1, 0, 2 });
|
||||
case 3:
|
||||
return new SimplePolynomial(new double[] { 0, -3, 0, 4 });
|
||||
case 4:
|
||||
return new SimplePolynomial(new double[] { 1, 0, -8, 0, 8 });
|
||||
case 5:
|
||||
return new SimplePolynomial(new double[] { 0, 5, 0, -20, 0, 16 });
|
||||
case 6:
|
||||
return new SimplePolynomial(new double[] { -1, 0, 1, 8, 0, -48, 0, 32 });
|
||||
case 7:
|
||||
return new SimplePolynomial(new double[] { 0, 7, 0, 56, 0, -112, 0, 64 });
|
||||
case 8:
|
||||
return new SimplePolynomial(new double[] { 1, 0, -32, 0, 160, 0, -256, 0, 128 });
|
||||
default:
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
}
|
||||
private static Polynomial BesselDenominatorPolynomial(int order)
|
||||
{
|
||||
// Polynomials in the square-rooted denominator of bessel gain functions
|
||||
// Obtained via magnitude calc of H(iw) of each Bessel H(s): https://en.wikipedia.org/wiki/Bessel_filter#The_transfer_function
|
||||
// at https://www.symbolab.com/solver/complex-numbers-magnitude-calculator/
|
||||
switch (order)
|
||||
{
|
||||
case 2:
|
||||
return new SimplePolynomial(new double[] { 9, 0, 3, 0, 1 });
|
||||
case 3:
|
||||
return new SimplePolynomial(new double[] { 225, 0, 45, 0, 6, 0, 1 });
|
||||
case 4:
|
||||
return new SimplePolynomial(new double[] { 11025, 0, 1575, 0, 135, 0, 10, 0, 1 });
|
||||
case 5:
|
||||
return new SimplePolynomial(new double[] { 893025, 0, 99225, 0, 6300, 0, 315, 0, 15, 0, 1 });
|
||||
case 6:
|
||||
return new SimplePolynomial(new double[] { 108056025, 0, 9823275, 0, 496125, 0, 18900, 0, 630, 0, 21, 0, 1 });
|
||||
case 7:
|
||||
return new SimplePolynomial(new double[] { 18261468225, 0, 1404728325, 0, 58939650, 0, 1819125, 0, 47250, 0, 1134, 0, 28, 0, 1 });
|
||||
case 8:
|
||||
return new SimplePolynomial(new double[] { 4108830350625, 0, 273922023375, 0, 9833098275, 0, 255405150, 0, 5457375, 0, 103950, 0, 1890, 0, 36, 0, 1 });
|
||||
default:
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user