/// <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; }