/// <summary>
/// Creates an IIR filter from zeros and poles. The zeros and poles must be made up of real and complex conjugate pairs.
/// </summary>
public IirFilter(ZeroPoleGain zeroPoleGain)
{
Debug.Assert(zeroPoleGain.Zeros.Length == zeroPoleGain.Poles.Length);
this.zeroPoleGain = zeroPoleGain;
transferFunction = FilterModifiers.ConvertSosToTransferFunction(sosGain);
sosGain = FilterModifiers.ConvertZeroPoleToSosFilter(zeroPoleGain);
this.sections = new FilterChain(sosGain.Sections);
this.order = sosGain.Sections.Sum(x => x.Order);
//Check whether this is linear phase
double[] b = transferFunction.B;
isLinearPhase = true;
for (int i = 0; i < b.Length; i++)
{
if (b[i] != b[b.Length - 1 - i] && b[i] != -b[b.Length - 1 - i])
{
isLinearPhase = false;
break;
}
}
return;
}
/// <summary>
/// Converts an analog prototype filter into a band-pass filter with a desired frequency. Similiar to lp2bp function in Matlab.
/// Based off of http://www.mikroe.com/eng/chapters/view/73/chapter-3-iir-filters/.
/// Uses the transform s -> (s^2 + wp1*wp2) / (s(wp2 - wp1)) where wp1 and wp2 are the new corner frequencies.
/// </summary>
/// <param name="freq1">The left corner frequency of the filter in radians.</param>
/// <param name="freq2">The right corner frequency of the filter in radians.</param>
/// <param name="prototype">The analog prototype filter.</param>
public static ZeroPoleGain AnalogPrototypeToBandPass(double freq1, double freq2, ZeroPoleGain prototype)
{
//Get the bandwidth
double bw = freq2 - freq1;
//Transformation for zeros and poles
Converter<Complex, Complex> zeroPoleTransform = x => ((bw*x) + ((bw*bw*x*x) - 4*freq1*freq2).SquareRoot)/2;
Converter<Complex, Complex> zeroPoleTransform2 = x => ((bw*x) - ((bw*bw*x*x) - 4*freq1*freq2).SquareRoot)/2;
//Transform the zeros and poles
Complex[] zeros = Array.ConvertAll(prototype.Zeros, zeroPoleTransform);
zeros = zeros.Concat(Array.ConvertAll(prototype.Zeros, zeroPoleTransform2)).ToArray();
Complex[] poles = Array.ConvertAll(prototype.Poles, zeroPoleTransform);
poles = poles.Concat(Array.ConvertAll(prototype.Poles, zeroPoleTransform2)).ToArray();
//Calculate the gain
int zeroCount = prototype.Zeros.Length;
int poleCount = prototype.Poles.Length;
double gain = prototype.Gain * Math.Pow(bw, poleCount - zeroCount);
//Add zeros
zeros = zeros.Concat(Enumerable.Repeat(Complex.Zero, poleCount - zeroCount)).ToArray();
return new ZeroPoleGain(gain, zeros, poles);
}
/// <summary>
/// Creates an FIR filter from zeros-pole-gain form. There must be no poles, and zeros must be made up of real and complex conjugate pairs.
/// </summary>
public FirFilter(ZeroPoleGain zeroPoleGain)
{
if (zeroPoleGain.Poles.Length > 0)
throw new ArgumentException("An FIR filter cannot have any poles.");
this.zeroPoleGain = zeroPoleGain;
transferFunction = FilterModifiers.ConvertZeroPoleToTransferFunction(zeroPoleGain);
this.order = zeroPoleGain.Zeros.Length;
this.data = (Queue<double>)Enumerable.Repeat(0, order); //!!
//Check whether this is linear phase
double[] b = transferFunction.B;
isLinearPhase = true;
for (int i = 0; i < b.Length; i++)
{
if (b[i] != b[b.Length - 1 - i] && b[i] != -b[b.Length - 1 - i])
{
isLinearPhase = false;
break;
}
}
return;
}
/// <summary>
/// Converts an analog prototype filter into a band-stop filter with a desired frequency. Similiar to lp2bs function in Matlab.
/// Based off of http://www.mikroe.com/eng/chapters/view/73/chapter-3-iir-filters/.
/// Uses the transform s -> s(ws2 - ws1) / (s^2 + ws1*ws2) where ws1 and ws2 are the new corner frequencies.
/// </summary>
/// <param name="freq1">The left corner frequency of the filter in radians.</param>
/// <param name="freq2">The right corner frequency of the filter in radians.</param>
/// <param name="prototype">The analog prototype filter.</param>
public static ZeroPoleGain AnalogPrototypeToBandStop(double freq1, double freq2, ZeroPoleGain prototype)
{
// Get the bandwidth
double bw = freq2 - freq1;
// Transformation for zeros and poles
Converter<Complex, Complex> zeroPoleTransform = x => (bw + (bw * bw - 4 * freq1 * freq2 * x * x).SquareRoot) / (2 * x);
Converter<Complex, Complex> zeroPoleTransform2 = x => (bw - (bw * bw - 4 * freq1 * freq2 * x * x).SquareRoot) / (2 * x);
// Transform the zeros and poles
Complex[] zeros = Array.ConvertAll(prototype.Zeros, zeroPoleTransform);
zeros = zeros.Concat(Array.ConvertAll(prototype.Zeros, zeroPoleTransform2)).ToArray();
Complex[] poles = Array.ConvertAll(prototype.Poles, zeroPoleTransform);
poles = poles.Concat(Array.ConvertAll(prototype.Poles, zeroPoleTransform2)).ToArray();
// Calculate the gain
Complex zeroMult = prototype.Zeros.Aggregate(Complex.One, (x, y) => x * -y);
Complex poleMult = prototype.Poles.Aggregate(Complex.One, (x, y) => x * -y);
double gain = (prototype.Gain * zeroMult / poleMult).Real; //Not sure why this isn't "gain = protoType.gain"
// Add zeros and poles
int zeroCount = prototype.Zeros.Length;
int poleCount = prototype.Poles.Length;
{
// Get position of new zeros and poles
Complex newZeroPoles = Complex.Eye * Math.Sqrt(freq1 * freq2);
// Get number of zeros to add
int zeroAddCount = poleCount - zeroCount;
if (zeroAddCount < 0)
zeroAddCount = 0;
// Add zeros
IEnumerable<Complex> newZeros = Enumerable.Repeat(newZeroPoles, zeroAddCount).Concat(Enumerable.Repeat(-newZeroPoles, zeroAddCount));
// Get number of poles to add
int poleAddCount = zeroCount - poleCount;
if (poleAddCount < 0)
poleAddCount = 0;
// Add poles
IEnumerable<Complex> newPoles = Enumerable.Repeat(newZeroPoles, poleAddCount).Concat(Enumerable.Repeat(-newZeroPoles, poleAddCount));
zeros = zeros.Concat(newZeros).ToArray();
poles = poles.Concat(newPoles).ToArray();
}
return new ZeroPoleGain(gain, zeros, poles);
}
/// <summary>
/// Converts two zeros and two poles into a second-order section. Assumes the final coefficients will be real.
/// </summary>
/// <param name="zeros"></param>
/// <param name="poles"></param>
public Sos(ZeroPoleGain zeroPoleGain)
{
Complex[] zeros = zeroPoleGain.Zeros;
Complex[] poles = zeroPoleGain.Poles;
//!!Does this really need to be true?
if (zeros.Length != poles.Length)
throw new Exception("There must be equal numbers of poles and zeros.");
int order = zeros.Length;
if (order != 1 && order != 2)
throw new Exception("There must be either one or two zeros and poles.");
//!! Make sure the zeros and poles are real or conjugate pairs
this.data = new double[order];
this.zeroPoleGain = zeroPoleGain;
this.transferFunction = ConvertZeroPoleToTransferFunction(this.zeroPoleGain);
return;
}
static IirFilter AnalogPrototypeToDigitalFilter(ZeroPoleGain analogPrototype, Pair<double,double> cornerFreqs, BandType bandType, IirFilterType filterType)
{
// Check that we have valid frequencies
bool isError = false;
isError |= (cornerFreqs.First < 0 || cornerFreqs.First > Math.PI);
if(bandType == BandType.BandPass || bandType == BandType.BandStop)
{
isError |= (cornerFreqs.Second < 0 || cornerFreqs.Second > Math.PI);
}
if (isError)
throw new Exception("Frequencies must be between 0 and PI inclusive");
//Prewarp the corner frequencies
//Assuming sampling frequency of 1/2 so that we don't need to account for sampling frequency when converting from analog to digital.
const double samplingFreq = 0.5;
Func<double, double> freqWarp = x => 2 * samplingFreq * Math.Tan(x/2);
Pair<double, double> warpedCornerFreqs = new Pair<double, double>();
warpedCornerFreqs.First = freqWarp(cornerFreqs.First);
warpedCornerFreqs.Second = freqWarp(cornerFreqs.Second);
// Set prototype to desired band
ZeroPoleGain analogFilter;
switch (bandType)
{
case BandType.LowPass:
analogFilter = FilterModifiers.AnalogPrototypeToLowPass(warpedCornerFreqs.First, analogPrototype);
break;
case BandType.HighPass:
analogFilter = FilterModifiers.AnalogPrototypeToHighPass(warpedCornerFreqs.First, analogPrototype);
break;
case BandType.BandPass:
analogFilter = FilterModifiers.AnalogPrototypeToBandPass(warpedCornerFreqs.First, warpedCornerFreqs.Second, analogPrototype);
break;
case BandType.BandStop:
analogFilter = FilterModifiers.AnalogPrototypeToBandStop(warpedCornerFreqs.First, warpedCornerFreqs.Second, analogPrototype);
break;
default: Debug.Assert(false); throw new Exception();
}
//Convert analog (continuous) filter to digital (discrete) filter
ZeroPoleGain digitalFilter;
digitalFilter = FilterModifiers.ConvertAnalogToDigital(analogFilter);
//Convert digital filter into second-order sections
SosGain sosGain = FilterModifiers.ConvertZeroPoleToSosFilter(digitalFilter);
//Create filter and return it
return new IirFilter(sosGain);
}
//!! Is there a more straightforward way to do this?
public static TransferFunction ConvertZeroPoleToTransferFunction(ZeroPoleGain zeroPoleGain)
{
SosGain sosGain = ConvertZeroPoleToSosFilter(zeroPoleGain);
return ConvertSosToTransferFunction(sosGain);
}
/// <summary>
/// Converts the zero-pole-gain filter into a filter with second-order sections. Similiar to zp2sos function in Matlab.
/// Based off of the Matlab function.
/// </summary>
/// <param name="filter">The filter in zero-pole-gain format.</param>
/// <returns>The filter as second-order sections.</returns>
public static SosGain ConvertZeroPoleToSosFilter(ZeroPoleGain zeroPoleGain)
{
// Make sure there are an equal number of poles and zeros
if (zeroPoleGain.Zeros.Length != zeroPoleGain.Poles.Length)
throw new Exception("There must be an equal number of poles and zeros.");
// Sort the zeros and poles
Func<Complex, bool> posImaginary = x => (!x.IsRealWithTolerance) && (x.Imaginary > 0);
Func<Complex, bool> negImaginary = x => (!x.IsRealWithTolerance) && (x.Imaginary < 0);
Func<Complex, bool> real = x => x.IsRealWithTolerance;
// Group the zeros into where they are on the plot
List<Complex> zerosPosImaginary = zeroPoleGain.Zeros.Where(posImaginary).ToList();
List<Complex> zerosNegImaginary = zeroPoleGain.Zeros.Where(negImaginary).ToList();
List<Complex> zerosReal = zeroPoleGain.Zeros.Where(real).ToList();
// Group the poles into where they are on the plot
List<Complex> polesPosImaginary = zeroPoleGain.Poles.Where(posImaginary).ToList();
List<Complex> polesNegImaginary = zeroPoleGain.Poles.Where(negImaginary).ToList();
List<Complex> polesReal = zeroPoleGain.Poles.Where(real).ToList();
//!! Make sure zeros and poles have all their pairs!
// Create second-order sections based on the following rules (similar to matlab:zp2sos):
// 1. Match poles closest to the unit circle with zeros closest to those poles.
// 2. Match the poles next closest to the unit circle with the zeros closest to those poles.
// 3. Continue until all of the poles and zeros are mtached.
// 4. Group real poles into sections with other real poles closest to them in absolute value (same for real zeros).
List<Sos> sosList = new List<Sos>();
Complex[] sortedPoles = polesPosImaginary.OrderByDescending(x => x.Magnitude).Concat(polesReal.OrderByDescending(x => x.Magnitude)).ToArray();
List<Complex> zeros = zerosPosImaginary.Concat(zerosReal).ToList();
int poleCount = 0;
int orderCount = 0;
while(poleCount < sortedPoles.Length)
{
List<Complex> sosPoles = new List<Complex>();
// Add first pole
sosPoles.Add(sortedPoles[poleCount]);
poleCount++;
orderCount++;
// Add second pole if there is one
if (orderCount < zeroPoleGain.Poles.Length)
{
switch (sosPoles[0].IsRealWithTolerance)
{
case false:
// Add complex conjugate
sosPoles.Add(sosPoles[0].Conjugate);
break;
case true:
// Add another real
sosPoles.Add(sortedPoles[poleCount]);
poleCount++;
break;
default:
Debug.Assert(false); throw new Exception();
}
orderCount++;
}
List<Complex> sosZeros = new List<Complex>();
Func<IEnumerable<Complex>, Func<Complex,double>, Complex> Max = (x,y) => x.Aggregate((w, z) => y(w) >= y(z) ? w : z);
Func<IEnumerable<Complex>, Func<Complex, double>, Complex> Min = (x, y) => x.Aggregate((w, z) => y(w) <= y(z) ? w : z);
// Add zero closest to first pole and remove it
Debug.Assert(zeros.Count > 0);
sosZeros.Add(Min(zeros, x => (sosPoles[0] - x).Magnitude));
zeros.Remove(sosZeros[0]);
if (sosPoles.Count == 1)
{
Debug.Assert(sosZeros[0].IsRealWithTolerance);
Debug.Assert(zeros.Count == 0);
}
// Add second zero and remove it
if (sosPoles.Count == 2)
{
switch (sosZeros[0].IsRealWithTolerance)
{
case false:
// Add complex conjugate
sosZeros.Add(sosZeros[0].Conjugate);
break;
case true:
// Add zero closest to the first zero
sosZeros.Add(Min(zeros, x => (sosZeros[0] - x).Magnitude));
// Remove the zero
zeros.Remove(sosZeros[1]);
break;
default:
Debug.Assert(false); throw new Exception();
}
}
//Create a second-order section
sosList.Add(new Sos(new ZeroPoleGain(1, sosZeros.ToArray(), sosPoles.ToArray())));
}
//Reverse sos sections so that the sections with the pole closest to the unit circle are at the end
sosList.Reverse();
// Return sos-gain form
return new SosGain(zeroPoleGain.Gain, sosList.ToArray());
}
/// <summary>
/// Converts the continuous analog filter to its discrete digital equivalent. Similiar to bilinear function in Matlab.
/// Based off of http://www.mikroe.com/eng/chapters/view/73/chapter-3-iir-filters/.
/// Uses the transform s = (z-1) / (z+1)
/// </summary>
/// <param name="analog">The analog filter in zero-pole-gain form.</param>
/// <param name="sampleRate">The sample rate of the filter in samples/second.</param>
public static ZeroPoleGain ConvertAnalogToDigital(ZeroPoleGain analog)
{
//The numerator order (number of zeros) cannot be higher than the denominator order (number of poles)
if (analog.Zeros.Length > analog.Poles.Length)
throw new Exception("The numerator order (number of zeros) cannot be higher than the denominator order (number of poles)");
//Get the order of the discrete filter
int filterOrder = analog.Poles.Length;
//Prewarp
//Don't need to do this because we warp the corner frequency earlier
//Remove all zeros at infinite
//Don't need to do this because we shouldn't have any zeros at infinite.
//Transformation for zeros and poles
Converter<Complex, Complex> zeroPoleTransform = x => (1 + x) / (1 - x);
//Transform the zeros and poles
Complex[] zeros = Array.ConvertAll(analog.Zeros, zeroPoleTransform);
Complex[] poles = Array.ConvertAll(analog.Poles, zeroPoleTransform);
//Calculate the gain
int zeroCount = analog.Zeros.Length;
int poleCount = analog.Poles.Length;
Complex zeroMult = analog.Zeros.Aggregate(Complex.One, (x, y) => x * (1 - y));
Complex poleMult = analog.Poles.Aggregate(Complex.One, (x, y) => x * (1 - y));
double gain = (analog.Gain * zeroMult / poleMult).Real;
//Add zeros
zeros = zeros.Concat(Enumerable.Repeat(-Complex.One, poleCount-zeroCount)).ToArray();
return new ZeroPoleGain(gain, zeros, poles);
}
/// <summary>
/// Converts an analog prototype filter into a low-pass filter with a desired corner frequency. Similiar to lp2lp function in Matlab.
/// Based off of http://www.mikroe.com/eng/chapters/view/73/chapter-3-iir-filters/.
/// Uses the transform s -> wc*s where wc is the new corner frequency.
/// </summary>
/// <param name="cornerFreq">The corner frequency of the filter in radians.</param>
/// <param name="prototype">The analog prototype filter.</param>
public static ZeroPoleGain AnalogPrototypeToLowPass(double cornerFreq, ZeroPoleGain prototype)
{
//Transformation for zeros and poles
Converter<Complex, Complex> zeroPoleTransform = x => cornerFreq * x;
//Transform the zeros and poles
Complex[] zeros = Array.ConvertAll(prototype.Zeros, zeroPoleTransform);
Complex[] poles = Array.ConvertAll(prototype.Poles, zeroPoleTransform);
//Calculate the gain
int zeroCount = prototype.Zeros.Length;
int poleCount = prototype.Poles.Length;
double gain = prototype.Gain * Math.Pow(cornerFreq, poleCount-zeroCount);
return new ZeroPoleGain(gain, zeros, poles);
}
/// <summary>
/// Converts an analog prototype filter into a high-pass filter with a desired corner frequency. Similiar to lp2hp function in Matlab.
/// Based off of http://www.mikroe.com/eng/chapters/view/73/chapter-3-iir-filters/.
/// Uses the transform s -> wc/s where wc is the new corner frequency.
/// </summary>
/// <param name="cornerFreq">The corner frequency of the filter in radians.</param>
/// <param name="prototype">The analog prototype filter.</param>
public static ZeroPoleGain AnalogPrototypeToHighPass(double cornerFreq, ZeroPoleGain prototype)
{
//Transformation for zeros and poles
Converter<Complex, Complex> zeroPoleTransform = x => cornerFreq / x;
//Transform the zeros and poles
Complex[] zeros = Array.ConvertAll(prototype.Zeros, zeroPoleTransform);
Complex[] poles = Array.ConvertAll(prototype.Poles, zeroPoleTransform);
//Calculate the gain
Complex zeroMult = prototype.Zeros.Aggregate(Complex.One, (x, y) => x * -y);
Complex poleMult = prototype.Poles.Aggregate(Complex.One, (x, y) => x * -y);
double gain = (prototype.Gain * zeroMult / poleMult).Real;
//Add zeros
int zeroCount = prototype.Zeros.Length;
int poleCount = prototype.Poles.Length;
zeros = zeros.Concat(Enumerable.Repeat(Complex.Zero, poleCount - zeroCount)).ToArray();
return new ZeroPoleGain(gain, zeros, poles);
}
/// <summary>
/// Converts the zeros and poles into transfer coefficients.
/// </summary>
/// <param name="zeros"></param>
/// <param name="poles"></param>
static TransferFunction ConvertZeroPoleToTransferFunction(ZeroPoleGain zeroPoleGain)
{
// Number of poles and zeros must be the same
Debug.Assert(zeroPoleGain.Zeros.Length == zeroPoleGain.Zeros.Length);
int order = zeroPoleGain.Zeros.Length;
// Can only have an order of 1 or 2
Debug.Assert(order == 1 || order == 2);
// Create our arrays of coefficients
double[] b = new double[order + 1];
double[] a = new double[order + 1];
// Calculate the coefficients
switch(order)
{
case 1:
{
Complex zero = zeroPoleGain.Zeros[0];
Complex pole = zeroPoleGain.Poles[0];
// Make sure zero and pole are both real
if (!zero.IsRealWithTolerance || !pole.IsRealWithTolerance)
throw new Exception("Zero and pole must be real in a first-order filter.");
// Convert pole and zero into transfer function coefficients
// (x - r1) -> x - r1
b[0] = 1;
b[1] = -zero.Real;
a[0] = 1;
a[1] = -pole.Real;
break;
}
case 2:
{
Complex[] zeros = zeroPoleGain.Zeros;
Complex[] poles = zeroPoleGain.Poles;
// Make sure we have complex conjugate pairs or real
bool zerosBothReal = zeros[0].IsRealWithTolerance && zeros[1].IsRealWithTolerance;
bool zerosConjugates = (zeros[0] + zeros[1]).IsRealWithTolerance;
bool polesBothReal = poles[0].IsRealWithTolerance && poles[1].IsRealWithTolerance;
bool polesConjugates = (poles[0] + poles[1]).IsRealWithTolerance;
if (!zerosBothReal && !zerosConjugates)
throw new Exception("Could not pair the zeros.");
if (!polesBothReal && !polesConjugates)
throw new Exception("Could not pair the poles.");
// Convert poles and zeros into transfer function coefficients
Helpers.RootsToPoly(zeros[0], zeros[1], out b[0], out b[1], out b[2]);
Helpers.RootsToPoly(poles[0], poles[1], out a[0], out a[1], out a[2]);
break;
}
default: Debug.Assert(false); throw new Exception();
}
// Incorporate gain into the numerator of the transfer function
b = Array.ConvertAll(b, x => zeroPoleGain.Gain * x);
return new TransferFunction(b, a);
}
/// <summary>
/// Gets the analog prototype of an n-order Elliptic filter (lowpass with wc = 1Hz). Similiar to ellipap function in Matlab.
/// </summary>
public static void Elliptic(int order, double passRipple, double stopRipple, out ZeroPoleGain zeroPoleGain)
{
if (order <= 0)
throw new ArgumentException("Filter order must be greater than zero.");
if (passRipple >= stopRipple)
throw new ArgumentException("Stopband attenuation must be greater than passband ripple.");
//Passband gain
double gp = Math.Pow(10, -passRipple / 20);
//Ripple factors
double ep = Math.Sqrt(Math.Pow(10, passRipple / 10) - 1);
double es = Math.Sqrt(Math.Pow(10, stopRipple / 10) - 1);
throw new NotImplementedException();
// Create zero-pole-gain form
//zeroPoleGain = new ZeroPoleGain(gain, zeros, poles);
//return;
}