public override WWUtil.LargeArray <double> FilterDo(WWUtil.LargeArray <double> inPcmLA)
{
System.Diagnostics.Debug.Assert(inPcmLA.LongLength == NumOfSamplesNeeded());
var inPcm = inPcmLA.ToArray();
// inPcmTをFFTしてinPcmFを得る。
// inPcmFを0で水増ししたデータoutPcmFを作って逆FFTしoutPcmを得る。
var inPcmF = mFFT.ForwardFft(inPcm);
var outPcmF = new WWComplex[UpsampleFftLength];
for (int i = 0; i < outPcmF.Length; ++i)
{
if (i <= FftLength / 2)
{
outPcmF[i] = inPcmF[i];
}
else if (UpsampleFftLength - FftLength / 2 <= i)
{
int pos = i + FftLength - UpsampleFftLength;
outPcmF[i] = inPcmF[pos];
}
else
{
outPcmF[i] = WWComplex.Zero();
}
}
inPcmF = null;
var outPcm = mFFT.InverseFft(outPcmF);
outPcmF = null;
return(new WWUtil.LargeArray <double>(outPcm));
}
private void ProcessCapturedData(BackgroundWorker bw, string folder)
{
// 録音したデータをrecPcmDataに入れる。
var recPcmData = new PcmDataLib.PcmData();
recPcmData.SetFormat(mStartTestingArgs.numChannels,
WasapiCS.SampleFormatTypeToUseBitsPerSample(mPref.RecSampleFormat),
WasapiCS.SampleFormatTypeToValidBitsPerSample(mPref.RecSampleFormat),
mPref.SampleRate,
PcmDataLib.PcmData.ValueRepresentationType.SInt,
mPcmPlay.NumFrames + 1); //< 再生したMaximum Length Sequenceのサイズよりも1サンプル多い。
recPcmData.SetSampleLargeArray(mCapturedPcmData);
// インパルス応答impulse[]を得る。
var recorded = recPcmData.GetDoubleArray(mStartTestingArgs.testChannel);
var decon = mMLSDecon.Deconvolution(recorded.ToArray());
var impulse = new double[decon.Length];
for (int i = 0; i < decon.Length; ++i)
{
impulse[i] = 2.0 * decon[decon.Length - 1 - i];
}
{
// ファイルに保存。
string path = string.Format("{0}/ImpulseResponse{1}_{2}.csv", folder, DateTime.Now.ToString("yyyyMMddHHmmss"), mCaptureCounter);
OutputImpulseToCsvFile(impulse, mPref.SampleRate, path);
}
++mCaptureCounter;
lock (mLock) {
mTimeDomainPlot.SetDiscreteTimeSequence(impulse, mPref.SampleRate);
}
// 周波数特性を計算する。
var fr = CalcFrequencyResponse(impulse);
{
// ファイルに保存。
string path = string.Format("{0}/FrequencyResponse{1}_{2}.csv", folder, DateTime.Now.ToString("yyyyMMddHHmmss"), mCaptureCounter);
OutputFrequencyResponseToCsvFile(fr, mPref.SampleRate, path);
}
lock (mLock) {
mFreqResponse.TransferFunction = (WWComplex z) => {
double θ = Math.Atan2(z.imaginary, z.real);
double freq01 = θ / Math.PI;
int pos = (int)(freq01 * fr.Length / 2);
if (pos < 0)
{
return(WWComplex.Zero());
}
return(fr[pos]);
};
}
// この後描画ができるタイミング(RecWorkerProgressChanged())でmTimeDomainPlot.Update()を呼ぶ。
bw.ReportProgress(10);
}
/// <summary>
/// ローパスフィルターの設計。
/// </summary>
/// <param name="mG0">0Hzのゲイン (dB)</param>
/// <param name="mGc">カットオフ周波数のゲイン (dB)</param>
/// <param name="mGs">ストップバンドのゲイン (dB)</param>
/// <param name="mFc">カットオフ周波数(Hz)</param>
/// <param name="mFs">ストップバンドの下限周波数(Hz)</param>
/// <returns></returns>
public bool DesignLowpass(double g0, double gc, double gs,
double fc, double fs, FilterType ft,
ApproximationBase.BetaType betaType)
{
// Hz → rad/s
double ωc = fc * 2.0 * Math.PI;
double ωs = fs * 2.0 * Math.PI;
// dB → linear
double h0 = Math.Pow(10, g0 / 20);
double hc = Math.Pow(10, gc / 20);
double hs = Math.Pow(10, gs / 20);
ApproximationBase filter;
switch (ft)
{
case FilterType.Butterworth:
filter = new ButterworthDesign(h0, hc, hs, ωc, ωs, betaType);
break;
case FilterType.Chebyshev:
filter = new ChebyshevDesign(h0, hc, hs, ωc, ωs, betaType);
break;
case FilterType.Pascal:
filter = new PascalDesign(h0, hc, hs, ωc, ωs, betaType);
break;
case FilterType.InverseChebyshev:
filter = new InverseChebyshevDesign(h0, hc, hs, ωc, ωs, betaType);
break;
case FilterType.Cauer:
filter = new CauerDesign(h0, hc, hs, ωc, ωs, betaType);
break;
default:
throw new NotImplementedException();
}
mOrder = filter.Order;
mNumeratorConstant = filter.TransferFunctionConstant();
// 伝達関数のポールの位置。
mPoleList.Clear();
for (int i = 0; i < filter.NumOfPoles(); ++i)
{
mPoleList.Add(filter.PoleNth(i));
}
// 伝達関数の零の位置。
mZeroList.Clear();
for (int i = 0; i < filter.NumOfZeroes(); ++i)
{
mZeroList.Add(filter.ZeroNth(i));
}
TransferFunction = (WWComplex s) => {
return(TransferFunctionValue(WWComplex.Div(s, ωc)));
};
PoleZeroPlotTransferFunction = (WWComplex s) => {
return(TransferFunctionValue(s));
};
{
// Unit Step Function
WWPolynomial.PolynomialAndRationalPolynomial H_s;
{
var unitStepRoots = new WWComplex[filter.NumOfPoles() + 1];
for (int i = 0; i < filter.NumOfPoles(); ++i)
{
var p = filter.PoleNth(i);
unitStepRoots[i] = p;
}
unitStepRoots[unitStepRoots.Length - 1] = WWComplex.Zero();
var numerCoeffs = new List <WWComplex>();
if (filter.NumOfZeroes() == 0)
{
numerCoeffs.Add(new WWComplex(mNumeratorConstant, 0));
}
else
{
numerCoeffs.AddRange(WWPolynomial.RootListToCoeffList(mZeroList.ToArray(), new WWComplex(mNumeratorConstant, 0)));
}
H_s = WWPolynomial.Reduction(numerCoeffs.ToArray(), unitStepRoots);
}
var H_fraction = WWPolynomial.PartialFractionDecomposition(H_s.numerCoeffList, H_s.denomRootList);
var H_integer = FirstOrderComplexRationalPolynomial.CreateFromCoeffList(H_s.coeffList);
UnitStepResponseFunction = (double t) => {
return(InverseLaplaceTransformValue(H_fraction, H_integer, t));
};
}
{
// 伝達関数 Transfer function
WWPolynomial.PolynomialAndRationalPolynomial H_s;
{
var H_Roots = new WWComplex[filter.NumOfPoles()];
for (int i = 0; i < filter.NumOfPoles(); ++i)
{
var p = filter.PoleNth(i);
H_Roots[i] = p;
}
var numerCoeffs = new List <WWComplex>();
if (filter.NumOfZeroes() == 0)
{
numerCoeffs.Add(new WWComplex(mNumeratorConstant, 0));
}
else
{
numerCoeffs.AddRange(WWPolynomial.RootListToCoeffList(mZeroList.ToArray(), new WWComplex(mNumeratorConstant, 0)));
}
H_s = WWPolynomial.Reduction(numerCoeffs.ToArray(), H_Roots);
}
var H_fraction = WWPolynomial.PartialFractionDecomposition(H_s.numerCoeffList, H_s.denomRootList);
var H_integer = FirstOrderComplexRationalPolynomial.CreateFromCoeffList(H_s.coeffList);
ImpulseResponseFunction = (double t) => {
return(InverseLaplaceTransformValue(H_fraction, H_integer, t));
};
#if true
mH_PFD = new List <FirstOrderComplexRationalPolynomial>();
for (int i = 0; i < H_fraction.Count; ++i)
{
mH_PFD.Add(H_fraction[i]);
if (1 == H_integer.Count && i == H_fraction.Count / 2 - 1)
{
mH_PFD.Add(H_integer[0]);
}
}
#else
mH_PFD = FirstOrderComplexRationalPolynomial.Add(H_fraction, H_integer);
#endif
TimeDomainFunctionTimeScale = 1.0 / filter.CutoffFrequencyHz();
if (NumOfZeroes() == 0)
{
// 共役複素数のペアを組み合わせて伝達関数の係数を全て実数にする。
// s平面のjω軸から遠い項から並べる。
mRealPolynomialList.Clear();
if ((H_fraction.Count() & 1) == 1)
{
// 奇数。
int center = H_fraction.Count() / 2;
mRealPolynomialList.Add(H_fraction[center]);
for (int i = 0; i < H_fraction.Count() / 2; ++i)
{
mRealPolynomialList.Add(WWPolynomial.Mul(
H_fraction[center - i - 1], H_fraction[center + i + 1]));
}
}
else
{
// 偶数。
int center = H_fraction.Count() / 2;
for (int i = 0; i < H_fraction.Count() / 2; ++i)
{
mRealPolynomialList.Add(WWPolynomial.Mul(
H_fraction[center - i - 1], H_fraction[center + i]));
}
}
#if true
System.Diagnostics.Debug.Assert(H_integer.Count == 0);
#else
{ // integral polynomialは全て実数係数の多項式。単に足す。
foreach (var p in H_integer)
{
mRealPolynomialList.Add(p);
}
}
#endif
}
else
{
// 共役複素数のペアを組み合わせて伝達関数の係数を全て実数にする。
// s平面のjω軸から遠い項から並べる。
// zeroListとPoleListを使って、実係数の2次多項式を作り、
// 伝達関数をこういう感じに実係数2次有理多項式の積の形にする。
// (s^2+c1)(s^2+c2) (s^2+c1) (s^2+c2)
// H(s) = ──────────────────────── ⇒ ──────────── * ────────────
// (s^2+a1s+b1)(s^2+a2s+b2) (s^2+a1s+b1) (s^2+a2s+b2)
mRealPolynomialList.Clear();
if ((mPoleList.Count & 1) == 1)
{
// 奇数。
int center = mPoleList.Count / 2;
mRealPolynomialList.Add(new FirstOrderComplexRationalPolynomial(
WWComplex.Zero(), WWComplex.Unity(),
WWComplex.Unity(), WWComplex.Minus(mPoleList[center])));
for (int i = 0; i < mPoleList.Count / 2; ++i)
{
var p0 = new FirstOrderComplexRationalPolynomial(
WWComplex.Unity(), WWComplex.Minus(mZeroList[center - i - 1]),
WWComplex.Unity(), WWComplex.Minus(mPoleList[center - i - 1]));
var p1 = new FirstOrderComplexRationalPolynomial(
WWComplex.Unity(), WWComplex.Minus(mZeroList[center + i]),
WWComplex.Unity(), WWComplex.Minus(mPoleList[center + i + 1]));
var p = WWPolynomial.Mul(p0, p1);
mRealPolynomialList.Add(p);
}
}
else
{
// 偶数。
int center = mPoleList.Count / 2;
for (int i = 0; i < mPoleList.Count / 2; ++i)
{
var p0 = new FirstOrderComplexRationalPolynomial(
WWComplex.Unity(), WWComplex.Minus(mZeroList[center - i - 1]),
WWComplex.Unity(), WWComplex.Minus(mPoleList[center - i - 1]));
var p1 = new FirstOrderComplexRationalPolynomial(
WWComplex.Unity(), WWComplex.Minus(mZeroList[center + i]),
WWComplex.Unity(), WWComplex.Minus(mPoleList[center + i]));
var p = WWPolynomial.Mul(p0, p1);
mRealPolynomialList.Add(p);
}
}
}
return(true);
}
}
public void AddZero(WWComplex zero)
{
mZeroCoordList.Add(zero);
}
public void AddPole(WWComplex pole)
{
mPoleCoordList.Add(pole);
}
private void UpdateZ()
{
double scale = mPoleZeroScale[comboBoxPoleZeroScale.SelectedIndex];
PoleZeroDispMode dispMode = (PoleZeroDispMode)comboBoxPoleZeroDispMode.SelectedIndex;
var im = new Image();
var bm = new WriteableBitmap(
512,
512,
96,
96,
dispMode == PoleZeroDispMode.Magnitude ? PixelFormats.Gray32Float : PixelFormats.Bgra32,
null);
var pxF = new float[bm.PixelHeight * bm.PixelWidth];
var pxBgra = new int[bm.PixelHeight * bm.PixelWidth];
im.Source = bm;
im.Stretch = Stretch.None;
im.HorizontalAlignment = HorizontalAlignment.Left;
im.VerticalAlignment = VerticalAlignment.Top;
int pos = 0;
for (int yI = 0; yI < bm.PixelHeight; yI++)
{
for (int xI = 0; xI < bm.PixelWidth; xI++)
{
double y = 2.6666666666666 / scale * (bm.PixelHeight / 2 - yI) / bm.PixelHeight;
double x = 2.6666666666666 / scale * (xI - bm.PixelWidth / 2) / bm.PixelHeight;
var z = new WWComplex(x, y);
var h = EvalH(z);
var hM = h.Magnitude();
if (hM < 0.1)
{
hM = 0.1;
}
float hL = (float)((Math.Log10(hM) + 1.0f) / 5.0f);
pxF[pos] = hL;
pxBgra[pos] = PhaseToBgra(h.Phase());
++pos;
}
}
switch (dispMode)
{
case PoleZeroDispMode.Magnitude:
bm.WritePixels(new Int32Rect(0, 0, bm.PixelWidth, bm.PixelHeight), pxF, bm.BackBufferStride, 0);
break;
case PoleZeroDispMode.Phase:
bm.WritePixels(new Int32Rect(0, 0, bm.PixelWidth, bm.PixelHeight), pxBgra, bm.BackBufferStride, 0);
break;
}
canvasZ.Children.Clear();
canvasZ.Children.Add(im);
double circleRadius = 192.0 * scale;
Ellipse unitCircle = new Ellipse {
Width = circleRadius * 2, Height = circleRadius * 2, Stroke = new SolidColorBrush {
Color = Colors.Black
}
};
unitCircle.Width = circleRadius * 2;
unitCircle.Height = circleRadius * 2;
canvasZ.Children.Add(unitCircle);
Canvas.SetLeft(unitCircle, 256.0 - circleRadius);
Canvas.SetTop(unitCircle, 256.0 - circleRadius);
Canvas.SetZIndex(unitCircle, 1);
}
private void UpdateFR()
{
foreach (var item in mLineList)
{
canvasFR.Children.Remove(item);
}
mLineList.Clear();
// calc frequency response
double [] frMagnitude = new double[FR_LINE_NUM];
double [] frPhase = new double[FR_LINE_NUM];
double maxMagnitude = 0.0f;
for (int i = 0; i < FR_LINE_NUM; ++i)
{
double theta = AngleFrequency(i);
var z = new WWComplex(Math.Cos(theta), Math.Sin(theta));
var h = EvalH(z);
frMagnitude[i] = h.Magnitude();
if (maxMagnitude < frMagnitude[i])
{
maxMagnitude = frMagnitude[i];
}
frPhase[i] = h.Phase();
}
if (maxMagnitude < float.Epsilon)
{
maxMagnitude = 1.0f;
}
// draw result
int sampleFrequency = SAMPLE_FREQS[comboBoxSampleFreq.SelectedIndex];
PhaseGraduation pg = mPhaseGraduationArray[comboBoxPhaseShift.SelectedIndex];
double phaseShift = pg.phaseShiftRad;
labelPhase180.Content = string.Format("{0}", 180 + pg.phaseShiftDeg);
labelPhase90.Content = string.Format("{0}", 90 + pg.phaseShiftDeg);
labelPhase0.Content = string.Format("{0}", 0 + pg.phaseShiftDeg);
labelPhaseM90.Content = string.Format("{0}", -90 + pg.phaseShiftDeg);
labelPhaseM180.Content = string.Format("{0}", -180 + pg.phaseShiftDeg);
switch (comboBoxFreqScale.SelectedIndex)
{
case (int)FreqScaleType.Linear:
labelFRMin.Visibility = Visibility.Visible;
labelFRMax.Visibility = System.Windows.Visibility.Hidden;
labelFRMin.Content = SampleFreqString(0);
labelFR0.Content = SampleFreqString(sampleFrequency * 1.0f / 8);
labelFR1.Content = SampleFreqString(sampleFrequency * 2.0f / 8);
labelFR2.Content = SampleFreqString(sampleFrequency * 3.0f / 8);
labelFR3.Content = SampleFreqString(sampleFrequency * 4.0f / 8);
lineFR0.X1 = lineFR0.X2 = FR_LINE_LEFT + FR_LINE_NUM * 1 / 4;
lineFR1.X1 = lineFR1.X2 = FR_LINE_LEFT + FR_LINE_NUM * 2 / 4;
lineFR2.X1 = lineFR2.X2 = FR_LINE_LEFT + FR_LINE_NUM * 3 / 4;
lineFR3.Visibility = System.Windows.Visibility.Hidden;
Canvas.SetLeft(labelFRMin, FR_LINE_LEFT - 10);
Canvas.SetLeft(labelFR0, lineFR0.X1 - 20);
Canvas.SetLeft(labelFR1, lineFR1.X1 - 20);
Canvas.SetLeft(labelFR2, lineFR2.X1 - 20);
Canvas.SetLeft(labelFR3, FR_LINE_LEFT + FR_LINE_NUM - 20);
break;
case (int)FreqScaleType.Logarithmic:
labelFRMin.Visibility = Visibility.Visible;
labelFRMax.Visibility = System.Windows.Visibility.Visible;
labelFRMin.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 0));
labelFR0.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 1));
labelFR1.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 2));
labelFR2.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 3));
labelFR3.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 4));
labelFRMax.Content = SampleFreqString(sampleFrequency / 2);
lineFR0.X1 = lineFR0.X2 = FR_LINE_LEFT + FR_LINE_NUM * LogFrequencyX(LogSampleFreq(sampleFrequency, 1));
lineFR1.X1 = lineFR1.X2 = FR_LINE_LEFT + FR_LINE_NUM * LogFrequencyX(LogSampleFreq(sampleFrequency, 2));
lineFR2.X1 = lineFR2.X2 = FR_LINE_LEFT + FR_LINE_NUM * LogFrequencyX(LogSampleFreq(sampleFrequency, 3));
lineFR3.X1 = lineFR3.X2 = FR_LINE_LEFT + FR_LINE_NUM * LogFrequencyX(LogSampleFreq(sampleFrequency, 4));
lineFR3.Visibility = System.Windows.Visibility.Visible;
Canvas.SetLeft(labelFRMin, FR_LINE_LEFT - 20);
Canvas.SetLeft(labelFR0, lineFR0.X1 - 20);
Canvas.SetLeft(labelFR1, lineFR1.X1 - 20);
Canvas.SetLeft(labelFR2, lineFR2.X1 - 20);
Canvas.SetLeft(labelFR3, lineFR3.X1 - 25);
Canvas.SetLeft(labelFRMax, FR_LINE_LEFT + FR_LINE_NUM);
break;
default:
System.Diagnostics.Debug.Assert(false);
break;
}
switch (comboBoxMagScale.SelectedIndex)
{
case (int)MagScaleType.Linear:
labelMagnitude.Content = "Magnitude";
labelFRMagMax.Content = string.Format("{0:0.00}", maxMagnitude);
labelFRMag2.Content = string.Format("{0:0.00}", maxMagnitude * 0.75);
labelFRMag1.Content = string.Format("{0:0.00}", maxMagnitude * 0.5);
labelFRMag0.Content = string.Format("{0:0.00}", maxMagnitude * 0.25);
labelFRMagMin.Content = string.Format("{0:0.00}", 0);
lineFRMag0.Y1 = lineFRMag0.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT * 1 / 4;
lineFRMag1.Y1 = lineFRMag1.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT * 2 / 4;
lineFRMag2.Y1 = lineFRMag2.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT * 3 / 4;
break;
case (int)MagScaleType.Logarithmic:
labelMagnitude.Content = "Magnitude (dB)";
labelFRMagMax.Content = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude));
labelFRMag2.Content = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude / 16));
labelFRMag1.Content = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude / 256));
labelFRMag0.Content = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude / 4096));
labelFRMagMin.Content = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude / 65536));
lineFRMag0.Y1 = lineFRMag0.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT * 1 / 4;
lineFRMag1.Y1 = lineFRMag1.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT * 2 / 4;
lineFRMag2.Y1 = lineFRMag2.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT * 3 / 4;
break;
default:
System.Diagnostics.Debug.Assert(false);
break;
}
var lastPosM = new Point();
var lastPosP = new Point();
for (int i = 0; i < FR_LINE_NUM; ++i)
{
Point posM = new Point();
Point posP = new Point();
double phase = frPhase[i] + phaseShift;
while (phase <= -Math.PI)
{
phase += 2.0 * Math.PI;
}
while (Math.PI < phase)
{
phase -= 2.0f * Math.PI;
}
posP = new Point(FR_LINE_LEFT + i, FR_LINE_YCENTER - FR_LINE_HEIGHT * phase / (2.0f * Math.PI));
switch (comboBoxMagScale.SelectedIndex)
{
case (int)MagScaleType.Linear:
posM = new Point(FR_LINE_LEFT + i, FR_LINE_BOTTOM - FR_LINE_HEIGHT * frMagnitude[i] / maxMagnitude);
break;
case (int)MagScaleType.Logarithmic:
posM = new Point(FR_LINE_LEFT + i,
FR_LINE_TOP + FR_LINE_HEIGHT * 20.0 * Math.Log10(frMagnitude[i] / maxMagnitude) / (20.0 * Math.Log10(1.0 / 65536)));
break;
default:
System.Diagnostics.Debug.Assert(false);
break;
}
if (1 <= i)
{
bool bDraw = true;
switch (comboBoxMagScale.SelectedIndex)
{
case (int)MagScaleType.Logarithmic:
if (FR_LINE_BOTTOM < posM.Y || FR_LINE_BOTTOM < lastPosM.Y)
{
bDraw = false;
}
break;
}
if (bDraw)
{
var lineM = new Line();
lineM.Stroke = Brushes.Blue;
LineSetX1Y1X2Y2(lineM, lastPosM.X, lastPosM.Y, posM.X, posM.Y);
mLineList.Add(lineM);
canvasFR.Children.Add(lineM);
}
}
if (2 <= i)
{
var lineP = new Line();
lineP.Stroke = Brushes.Red;
LineSetX1Y1X2Y2(lineP, lastPosP.X, lastPosP.Y, posP.X, posP.Y);
mLineList.Add(lineP);
canvasFR.Children.Add(lineP);
}
lastPosP = posP;
lastPosM = posM;
}
}
/// <summary>
/// Design of Discrete-time IIR filters from continuous-time filter using impulse invariance method
/// A. V. Oppenheim, R. W. Schafer, Discrete-Time Signal Processing, 3rd Ed, Prentice Hall, 2009
/// pp. 526 - 529
///
/// minimumPhase==true : 多項式の積の形の伝達関数が出てくる。
/// minimumPhase==false : mixed phaseとなり、多項式の和の形の伝達関数が出てくる。
/// </summary>
public ImpulseInvarianceMethod(List <FirstOrderComplexRationalPolynomial> H_s,
double ωc, double sampleFreq, bool minimumPhase)
{
mMinimumPhase = minimumPhase;
mSamplingFrequency = sampleFreq;
/*
* H_sはノーマライズされているので、戻す。
*
* b b * ωc
* ────────── = ────────────
* s/ωc - a s - a * ωc
*/
double td = 1.0 / sampleFreq;
mComplexHzList.Clear();
foreach (var pS in H_s)
{
WWComplex sktd;
if (pS.DenomDegree() == 0)
{
System.Diagnostics.Debug.Assert(pS.D(0).EqualValue(WWComplex.Unity()));
// ?
// a * u[t] → exp^(a)
sktd = WWComplex.Minus(WWComplex.Mul(WWComplex.Unity(), ωc * td));
}
else
{
sktd = WWComplex.Minus(WWComplex.Mul(pS.D(0), ωc * td));
}
// e^{sktd} = e^{real(sktd)} * e^{imag{sktd}}
// = e^{real(sktd)} * ( cos(imag{sktd}) + i*sin(imag{sktd})
var expsktd = new WWComplex(
Math.Exp(sktd.real) * Math.Cos(sktd.imaginary),
Math.Exp(sktd.real) * Math.Sin(sktd.imaginary));
// pZは、z^-1についての式。
// y[1] : z^{-1}の項
// y[0] : 定数項
var pZ = new FirstOrderComplexRationalPolynomial(
WWComplex.Zero(), WWComplex.Mul(pS.N(0), ωc * td),
WWComplex.Minus(expsktd), WWComplex.Unity());
mComplexHzList.Add(pZ);
}
mH_z = new HighOrderComplexRationalPolynomial(mComplexHzList[0]);
for (int i = 1; i < mComplexHzList.Count; ++i)
{
mH_z = WWPolynomial.Add(mH_z, mComplexHzList[i]);
}
if (mMinimumPhase)
{
// ミニマムフェーズにする。
var numerPoly = mH_z.NumerPolynomial();
var aCoeffs = new double[numerPoly.Degree + 1];
for (int i = 0; i < aCoeffs.Length; ++i)
{
aCoeffs[i] = numerPoly.C(i).real;
}
var rpoly = new JenkinsTraubRpoly();
bool result = rpoly.FindRoots(new RealPolynomial(aCoeffs));
if (!result)
{
Console.WriteLine("Error: rpoly.FindRoots failed!");
throw new ArgumentException();
}
// ポールの位置 = mHzListの多項式の分母のリストから判明する。
var poles = new WWComplex[mComplexHzList.Count];
for (int i = 0; i < mComplexHzList.Count; ++i)
{
var p = mComplexHzList[i];
Console.WriteLine(" {0} {1}", i, p);
poles[i] = WWComplex.Div(p.D(0), p.D(1)).Minus();
}
System.Diagnostics.Debug.Assert(poles.Length == rpoly.NumOfRoots() + 1);
var zeroes = new WWComplex[rpoly.NumOfRoots()];
for (int i = 0; i < rpoly.NumOfComplexRoots() / 2; ++i)
{
var p0 = rpoly.ComplexRoot(i * 2);
var p1 = rpoly.ComplexRoot(i * 2 + 1);
if (p0.Magnitude() < 1.0)
{
// 単位円の外側にゼロがあるのでconjugate reciprocalにする。
p0 = p0.ConjugateReciprocal();
p1 = p1.ConjugateReciprocal();
}
zeroes[i * 2] = p0;
zeroes[i * 2 + 1] = p1;
}
for (int i = 0; i < rpoly.NumOfRealRoots(); ++i)
{
var p = rpoly.RealRoot(i);
if (p.Magnitude() < 1.0)
{
// 単位円の外側にゼロがあるのでconjugate reciprocalにする。
p = p.ConjugateReciprocal();
}
zeroes[i + rpoly.NumOfComplexRoots()] = p;
}
mComplexHzList.Clear();
// ポールと零のペアを、係数を実数化出来るように対称に並べる。
// ポールと共役のペアになっていて、ペアは例えばn=5の時
// C0+ C1+ R C1- C0- のように並んでいる。
// 零は共役のペアになっていて、ペアは例えばn=5の時
// C0+ C0- C1+ C1- R のように並んでいる。
// mComplexHzListは C0+ C0- C1+ C1- R のように並べる。
for (int i = 0; i < poles.Length / 2; ++i)
{
var cP = new FirstOrderComplexRationalPolynomial(
WWComplex.Unity(), zeroes[i * 2].Minus(),
WWComplex.Unity(), poles[i].Minus());
mComplexHzList.Add(cP);
var cM = new FirstOrderComplexRationalPolynomial(
WWComplex.Unity(), zeroes[i * 2 + 1].Minus(),
WWComplex.Unity(), poles[poles.Length - 1 - i].Minus());
mComplexHzList.Add(cM);
}
{
var p = new FirstOrderComplexRationalPolynomial(
WWComplex.Zero(), WWComplex.Unity(),
WWComplex.Unity(), poles[poles.Length / 2].Minus());
mComplexHzList.Add(p);
}
// 0Hz (z^-1 == 1)のときのゲインが1になるようにする。
WWComplex gain = WWComplex.Unity();
foreach (var p in mComplexHzList)
{
gain = WWComplex.Mul(gain, p.Evaluate(WWComplex.Unity()));
}
mComplexHzList[mComplexHzList.Count - 1] =
mComplexHzList[mComplexHzList.Count - 1].ScaleNumeratorCoeffs(1.0 / gain.real);
var gainC = WWComplex.Unity();
foreach (var p in mComplexHzList)
{
gainC = WWComplex.Mul(gainC, p.Evaluate(WWComplex.Unity()));
}
// 係数が全て実数のmRealHzListを作成する。
mRealHzList.Clear();
for (int i = 0; i < mComplexHzList.Count / 2; ++i)
{
var p0 = mComplexHzList[i * 2];
var p1 = mComplexHzList[i * 2 + 1];
var p = WWPolynomial.Mul(p0, p1).ToRealPolynomial();
mRealHzList.Add(p);
}
mRealHzList.Add(new RealRationalPolynomial(
new double[] { 1.0 / gain.real },
new double[] { -poles[poles.Length / 2].real, 1.0 }));
var gainR = 1.0;
foreach (var p in mRealHzList)
{
gainR *= p.Evaluate(1.0);
}
// mH_zを作り直す。
var newNumerCoeffs = WWPolynomial.RootListToCoeffList(zeroes, WWComplex.Unity());
var poleCoeffs = mH_z.DenomPolynomial().ToArray();
mH_z = new HighOrderComplexRationalPolynomial(newNumerCoeffs, poleCoeffs);
var gain2 = mH_z.Evaluate(WWComplex.Unity());
for (int i = 0; i < newNumerCoeffs.Length; ++i)
{
newNumerCoeffs[i] = WWComplex.Mul(newNumerCoeffs[i], 1.0 / gain2.Magnitude());
}
mH_z = new HighOrderComplexRationalPolynomial(newNumerCoeffs, poleCoeffs);
var gain3 = mH_z.Evaluate(WWComplex.Unity());
Console.WriteLine(mH_z.ToString("z", WWMathUtil.SymbolOrder.Inverted));
}
else
{
// mixed-phase
// mComplexHzListは多項式の和の形になっている。
// 0Hz (z^-1 == 1)のときのゲインが1になるようにする。
WWComplex gain = WWComplex.Zero();
foreach (var p in mComplexHzList)
{
gain = WWComplex.Add(gain, p.Evaluate(WWComplex.Unity()));
}
mComplexHzList[mComplexHzList.Count / 2] =
mComplexHzList[mComplexHzList.Count / 2].ScaleNumeratorCoeffs(1.0 / gain.real);
var gainC = WWComplex.Zero();
foreach (var p in mComplexHzList)
{
gainC = WWComplex.Add(gainC, p.Evaluate(WWComplex.Unity()));
}
// 係数が全て実数のmRealHzListを作成する。
// mRealHzListは、多項式の和を表現する。
mRealHzList.Clear();
for (int i = 0; i < mComplexHzList.Count / 2; ++i)
{
var p0 = mComplexHzList[i];
var p1 = mComplexHzList[mComplexHzList.Count - 1 - i];
var p = WWPolynomial.Add(p0, p1).ToRealPolynomial();
mRealHzList.Add(p);
}
{
var p = mComplexHzList[mComplexHzList.Count / 2];
mRealHzList.Add(new RealRationalPolynomial(
new double[] { p.N(0).real },
new double[] { p.D(0).real, p.D(1).real }));
}
var gainR = 0.0;
foreach (var p in mRealHzList)
{
gainR += p.Evaluate(1.0);
}
Console.WriteLine("gainR={0}", gainR);
}
TransferFunction = (WWComplex z) => { return(TransferFunctionValue(z)); };
// ポールの位置 = mHzListの多項式の分母のリストから判明する。
mPoleArray = new WWComplex[mComplexHzList.Count];
for (int i = 0; i < mComplexHzList.Count; ++i)
{
var p = mComplexHzList[i];
Console.WriteLine(" {0} {1}", i, p);
mPoleArray[i] = WWComplex.Div(p.D(0), p.D(1)).Minus();
}
{
// 零の位置を計算する。
// 合体したH(z)の分子の実係数多項式の根が零の位置。
var poly = mH_z.NumerPolynomial();
var coeffs = new double[poly.Degree + 1];
for (int i = 0; i < coeffs.Length; ++i)
{
coeffs[i] = poly.C(i).real;
}
var rf = new JenkinsTraubRpoly();
bool b = rf.FindRoots(new RealPolynomial(coeffs));
if (b)
{
Console.WriteLine("■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■");
Console.WriteLine("polynomial degree = {0}, {1}/{2}", mH_z.Degree(), mH_z.NumerDegree(), mH_z.DenomDegree());
Console.WriteLine("Hz={0}", mH_z.ToString("z^-1"));
mZeroArray = rf.RootArray();
foreach (var r in mZeroArray)
{
Console.WriteLine(" zero at {0}", WWComplex.Reciprocal(r));
}
}
}
}
public override WWUtil.LargeArray <double> FilterDo(WWUtil.LargeArray <double> inPcmLA)
{
System.Diagnostics.Debug.Assert(inPcmLA.LongLength <= NumOfSamplesNeeded());
var inPcm = inPcmLA.ToArray();
var fft = new WWRadix2Fft(FFT_LEN);
// Overlap and add continuous FFT
var inTime = new WWComplex[FFT_LEN];
for (int i = 0; i < inPcm.Length; ++i)
{
inTime[i] = new WWComplex(inPcm[i], 0);
}
// FFTでinTimeをinFreqに変換
var inFreq = fft.ForwardFft(inTime);
inTime = null;
// FFT後、フィルターHの周波数ドメインデータを掛ける
var mulFreq = WWComplex.Mul(inFreq, mFilterFreq);
inFreq = null;
// inFreqをIFFTしてoutTimeに変換
var outTime = fft.InverseFft(mulFreq);
mulFreq = null;
double [] outReal;
if (mFirstFilterDo)
{
// 最初のFilterDo()のとき、フィルタの遅延サンプル数だけ先頭サンプルを削除する。
outReal = new double[NumOfSamplesNeeded() - FILTER_DELAY];
for (int i = 0; i < outReal.Length; ++i)
{
outReal[i] = outTime[i + FILTER_DELAY].real;
}
mFirstFilterDo = false;
}
else
{
outReal = new double[NumOfSamplesNeeded()];
for (int i = 0; i < outReal.Length; ++i)
{
outReal[i] = outTime[i].real;
}
}
// 前回のIFFT結果の最後のFILTER_LENGTH-1サンプルを先頭に加算する
if (null != mIfftAddBuffer)
{
for (int i = 0; i < mIfftAddBuffer.Length; ++i)
{
outReal[i] += mIfftAddBuffer[i];
}
}
// 今回のIFFT結果の最後のFILTER_LENGTH-1サンプルをmIfftAddBufferとして保存する
mIfftAddBuffer = new double[FILTER_LENP1];
for (int i = 0; i < mIfftAddBuffer.Length; ++i)
{
mIfftAddBuffer[i] = outTime[outTime.Length - mIfftAddBuffer.Length + i].real;
}
outTime = null;
return(new WWUtil.LargeArray <double>(outReal));
}
public void ConvolutionBruteForceTest()
{
var target = new WWConvolution();
var h = new WWComplex [] { new WWComplex(2, 0), WWComplex.Unity() };
var x = new WWComplex[] { WWComplex.Unity(), WWComplex.Unity() };
var actual = target.ConvolutionBruteForce(h, x);
var expected = new WWComplex[] { new WWComplex(2, 0), new WWComplex(3, 0), WWComplex.Unity() };
Assert.IsTrue(Compare(actual, expected));
}
public void FillZeroes()
{
Fill(WWComplex.Zero());
}
public WWComplex[] Run(string path)
{
System.Drawing.Bitmap bm = new System.Drawing.Bitmap(path);
int width = bm.Width;
int height = bm.Height;
// 0を表す中央の線の位置とグラフの線の位置を調べる。
var positions = new GraphPositions[width];
var histogram = new int[height];
for (int x = 0; x < width; ++x)
{
// 上から調べる
for (int y = 0; y < height; ++y)
{
var c = bm.GetPixel(x, y);
float b = c.GetBrightness();
if (b < 0.5f)
{
positions[x].upper = y;
++histogram[y];
break;
}
}
// 下から調べる
for (int y = height - 1; 0 <= y; --y)
{
var c = bm.GetPixel(x, y);
float b = c.GetBrightness();
if (b < 0.5f)
{
positions[x].lower = y;
++histogram[y];
break;
}
}
}
int minValue = -1;
for (int y = 0; y < height; ++y)
{
if (0 < histogram[y])
{
minValue = y;
break;
}
}
int maxValue = height;
for (int y = height - 1; 0 <= y; --y)
{
if (0 < histogram[y])
{
maxValue = y;
break;
}
}
int zeroPosition = 0;
int histogramMax = 0;
for (int y = 0; y < height; ++y)
{
if (histogramMax < histogram[y])
{
zeroPosition = y;
histogramMax = histogram[y];
}
}
// 中央の線の位置 = zeroPosition
// 最大値 maxValue
// 最小値 minValue
double center = (maxValue + minValue) / 2.0;
double magnitude = (maxValue - minValue) / 2.0;
var waveForm = new WWComplex[width];
for (int x = 0; x < width; ++x)
{
int iv = positions[x].upper;
if (zeroPosition == iv)
{
iv = positions[x].lower;
}
double v = (center - iv) / magnitude;
waveForm[x] = new WWComplex(v, 0);
Console.WriteLine("{0} {1}", x, v);
}
WWComplex [] freqResponse;
WWDftCpu.Dft1d(waveForm, out freqResponse);
return(freqResponse);
}
/// <summary>
/// Addし終わったら呼ぶ。
/// </summary>
public void Calc()
{
// mComplexHzListに1次の関数が入っている。
// 係数が全て実数のmRealHzListを作成する。
// mRealHzListは、多項式の和を表現する。
mRealHzList.Clear();
for (int i = 0; i < mComplexHzList.Count / 2; ++i)
{
var p0 = mComplexHzList[i];
var p1 = mComplexHzList[mComplexHzList.Count - 1 - i];
var p = WWPolynomial.Add(p0, p1).ToRealPolynomial();
mRealHzList.Add(p);
}
if ((mComplexHzList.Count & 1) == 1)
{
// 1次の項。
var p = mComplexHzList[mComplexHzList.Count / 2];
mRealHzList.Add(new RealRationalPolynomial(
new double[] { p.N(0).real, p.N(1).real },
new double[] { p.D(0).real, p.D(1).real }));
}
var stopbandGain = WWComplex.Zero();
if (mFilterType == FilterType.Lowpass)
{
foreach (var p in mRealHzList)
{
stopbandGain = WWComplex.Add(stopbandGain, p.Evaluate(WWComplex.Unity()));
}
}
else
{
foreach (var p in mRealHzList)
{
stopbandGain = WWComplex.Add(stopbandGain, p.Evaluate(new WWComplex(-1, 0)));
}
}
// DCゲインが1になるようにHzをスケールする。
for (int i = 0; i < mRealHzList.Count; ++i)
{
var p = mRealHzList[i];
mRealHzList[i] = p.ScaleNumerCoeffs(1.0 / stopbandGain.real);
}
stopbandGain = WWComplex.Zero();
foreach (var p in mRealHzList)
{
stopbandGain = WWComplex.Add(stopbandGain, p.Evaluate(WWComplex.Unity()));
}
Console.WriteLine("DC gain={0}", stopbandGain);
TransferFunction = (WWComplex z) => { return(TransferFunctionValue(z)); };
mHzCombined = new RealRationalPolynomial(mRealHzList[0]);
for (int i = 1; i < mRealHzList.Count; ++i)
{
var p = mRealHzList[i];
mHzCombined = WWPolynomial.Add(mHzCombined, p);
}
mHzCombined = mHzCombined.ScaleAllCoeffs(1.0 / mHzCombined.D(0));
}
/// <summary>
/// バターワースフィルターのFFT coeffs
/// </summary>
/// <param name="filterSlopeDbOct">1次 = 6(dB/oct), 2次 = 12(dB/oct)</param>
/// <returns></returns>
public static WWComplex[] Design(int sampleRate, double cutoffFrequency, int fftLength, int filterSlopeDbOct)
{
int filterLenP1 = fftLength / 4;
int filterLength = filterLenP1 - 1;
var fromF = new WWComplex[filterLenP1];
double orderX2 = 2.0 * (filterSlopeDbOct / 6.0);
double cutoffRatio = cutoffFrequency / (sampleRate / 2);
// フィルタのF特
fromF[0] = WWComplex.Unity();
for (int i = 1; i <= filterLenP1 / 2; ++i)
{
double omegaRatio = i * (1.0 / (filterLenP1 / 2));
double v = Math.Sqrt(1.0 / (1.0 + Math.Pow(omegaRatio / cutoffRatio, orderX2)));
if (Math.Abs(v) < Math.Pow(0.5, 24))
{
v = 0.0;
}
fromF[i] = new WWComplex(v, 0);
}
for (int i = 1; i < filterLenP1 / 2; ++i)
{
fromF[filterLenP1 - i] = fromF[i];
}
// IFFTでfromFをfromTに変換
WWComplex[] fromT;
{
var fft = new WWRadix2Fft(filterLenP1);
fromT = fft.ForwardFft(fromF);
double compensation = 1.0 / (filterLenP1 * cutoffRatio);
for (int i = 0; i < filterLenP1; ++i)
{
fromT[i] = new WWComplex(
fromT[i].real * compensation,
fromT[i].imaginary * compensation);
}
}
fromF = null;
// fromTの中心がFILTER_LENGTH/2番に来るようにする。
// delayT[0]のデータはfromF[FILTER_LENGTH/2]だが、非対称になるので入れない
// このフィルタの遅延はFILTER_LENGTH/2サンプルある
var delayT = new WWComplex[filterLenP1];
delayT[0] = WWComplex.Zero();
for (int i = 1; i < filterLenP1 / 2; ++i)
{
delayT[i] = fromT[i + filterLenP1 / 2];
}
for (int i = 0; i < filterLenP1 / 2; ++i)
{
delayT[i + filterLenP1 / 2] = fromT[i];
}
fromT = null;
// Kaiser窓をかける
var w = WWWindowFunc.KaiserWindow(filterLenP1 + 1, 9.0);
for (int i = 0; i < filterLenP1; ++i)
{
delayT[i] = WWComplex.Mul(delayT[i], w[i]);
}
var delayTL = new WWComplex[fftLength];
for (int i = 0; i < delayT.Length; ++i)
{
delayTL[i] = delayT[i];
}
for (int i = delayT.Length; i < delayTL.Length; ++i)
{
delayTL[i] = WWComplex.Zero();
}
delayT = null;
// できたフィルタをFFTする
WWComplex[] delayF;
{
var fft = new WWRadix2Fft(fftLength);
delayF = fft.ForwardFft(delayTL);
for (int i = 0; i < fftLength; ++i)
{
delayF[i] = WWComplex.Mul(delayF[i], cutoffRatio);
}
}
delayTL = null;
return(delayF);
}
