/// <summary>
/// Generate column from initial matrix to work array
/// </summary>
/// <param name="a">Initial matrix</param>
/// <param name="row">The first row</param>
/// <param name="column">Column index</param>
/// <returns>Generated vector</returns>
static Complex[] GenerateColumn(Matrix <Complex> a, int row, int column)
{
var ru = a.RowCount - row;
var u = new Complex[ru];
for (var i = row; i < a.RowCount; i++)
{
u[i - row] = a.At(i, column);
a.At(i, column, 0.0);
}
var norm = u.Aggregate(Complex.Zero, (current, t) => current + (t.Magnitude * t.Magnitude));
norm = norm.SquareRoot();
if (row == a.RowCount - 1 || norm.Magnitude == 0)
{
a.At(row, column, -u[0]);
u[0] = Constants.Sqrt2;
return(u);
}
if (u[0].Magnitude != 0.0)
{
norm = norm.Magnitude * (u[0] / u[0].Magnitude);
}
a.At(row, column, -norm);
for (var i = 0; i < ru; i++)
{
u[i] /= norm;
}
u[0] += 1.0;
var s = (1.0 / u[0]).SquareRoot();
for (var i = 0; i < ru; i++)
{
u[i] = u[i].Conjugate() * s;
}
return(u);
}
/// <summary>
/// Creates a nth-order low-pass or high-pass Type I FIR filter with a specified window.
/// </summary>
/// <param name="freq">Corner frequency in radians.</param>
/// <param name="order">Filter order.</param>
/// <param name="window">Window type.</param>
/// <param name="lowpass">True specifies low-pass, false specifies high-pass.</param>
/// <returns></returns>
public static FirFilter CreateFirFilter(double freq, int order, double[] window, bool lowpass)
{
if ((order % 2 == 1) && !lowpass)
throw new ArgumentException("Cannot make a Type I highpass filter with an odd filter order");
int filterLength = order+1;
if(window.Length != filterLength)
throw new ArgumentException("Window must have a length of order + 1");
//Create ideal impulse response
double[] idealImpulseResponse = new double[filterLength];
if (lowpass)
{
for (int i = 0; i < idealImpulseResponse.Length; i++)
{
idealImpulseResponse[i] = Sinc(freq * (i - filterLength / 2)) * freq / Math.PI;
}
}
else
{
for (int i = 0; i < idealImpulseResponse.Length; i++)
{
idealImpulseResponse[i] = Sinc(Math.PI * (i-filterLength/2)) - Sinc(freq * (i - filterLength / 2)) * freq / Math.PI;
}
}
//Make sure the ideal impulse response is symmetric
for (int i = 0; i < idealImpulseResponse.Length; i++)
{
Debug.Assert(idealImpulseResponse[i] == idealImpulseResponse[idealImpulseResponse.Length - 1 - i]);
}
//Window ideal impulse response
double[] impulseResponse = (double[])idealImpulseResponse.Zip(window, (x, y) => x * y);
//Scale filter to have pass-band approximately equal to one
double[] scaledImpulseResponse;
if (lowpass)
{
//Scale everything so we have unity gain at the DC offset (0 Hz)
double dcValue = impulseResponse.Sum();
scaledImpulseResponse = Array.ConvertAll(impulseResponse, x => x / dcValue);
}
else
{
//Scale everything so we have unity gain at Nyquist frequency (Pi Hz)
double f0 = Math.PI;
Complex[] freqs = new Complex[filterLength];
for(int i=0; i<freqs.Length; i++)
{
freqs[i] = Complex.FromPolar(impulseResponse[i], -Math.PI*i*f0);
}
double nyquistValue = freqs.Aggregate((x,y) => x + y).Magnitude;
scaledImpulseResponse = Array.ConvertAll(impulseResponse, x => x / nyquistValue);
}
//Make sure the ideal impulse response is symmetric
for (int i = 0; i < idealImpulseResponse.Length; i++)
{
Debug.Assert(idealImpulseResponse[i] == idealImpulseResponse[idealImpulseResponse.Length - 1 - i]);
}
return new FirFilter(new TransferFunction(scaledImpulseResponse, new[] { 1.0 }));
}
/// <summary>
/// Generate column from initial matrix to work array
/// </summary>
/// <param name="a">Initial matrix</param>
/// <param name="row">The first row</param>
/// <param name="column">Column index</param>
/// <returns>Generated vector</returns>
static Complex[] GenerateColumn(Matrix<Complex> a, int row, int column)
{
var ru = a.RowCount - row;
var u = new Complex[ru];
for (var i = row; i < a.RowCount; i++)
{
u[i - row] = a.At(i, column);
a.At(i, column, 0.0);
}
var norm = u.Aggregate(Complex.Zero, (current, t) => current + (t.Magnitude*t.Magnitude));
norm = norm.SquareRoot();
if (row == a.RowCount - 1 || norm.Magnitude == 0)
{
a.At(row, column, -u[0]);
u[0] = Constants.Sqrt2;
return u;
}
if (u[0].Magnitude != 0.0)
{
norm = norm.Magnitude*(u[0]/u[0].Magnitude);
}
a.At(row, column, -norm);
for (var i = 0; i < ru; i++)
{
u[i] /= norm;
}
u[0] += 1.0;
var s = (1.0/u[0]).SquareRoot();
for (var i = 0; i < ru; i++)
{
u[i] = u[i].Conjugate()*s;
}
return u;
}
