/// <summary>
/// 連続時間フィルターの伝達関数を離散時間フィルターの伝達関数にBilinear transformする。
/// Discrete-time signal processing 3rd ed. pp.533
/// Benoit Boulet, 信号処理とシステムの基礎 pp.681-682
/// 三谷政昭, ディジタル・フィルタ理論&設計入門 pp.193
/// </summary>
/// <param name="ps">連続時間フィルターの伝達関数</param>
/// <returns>離散時間フィルターの伝達関数</returns>
public FirstOrderComplexRationalPolynomial StoZ(FirstOrderComplexRationalPolynomial ps)
{
/*
* n1s + n0
* ps = ──────────
* d1s + d0
*
* z^{-1} = zM とする。
*
* Bilinear transform:
* 2 1-zM
* s → ─── * ──────
* Td 1+zM
*
* 2/Td = kとすると以下のように書ける。
*
* k(1-zM)
* s → ───────
* 1+zM
*
* k(1-zM) n1*k(1-zM) + n0(1+zM)
* n1 * ─────── + n0 ─────────────────────
* 1+zM 1+zM n1*k(1-zM) + n0(1+zM) (n0-n1*k)zM + n0+n1*k
* pz = ─────────────────── = ──────────────────────── = ───────────────────── = ─────────────────────
* k(1-zM) d1*k(1-zM) + d0(1+zM) d1*k(1-zM) + d0(1+zM) (d0-d1*k)zM + d0+d1*k
* d1 * ─────── + d0 ─────────────────────
* 1+zM 1+zM
*
*/
// 都合により、投入されるアナログフィルターの伝達関数psは、
// s' == s / ωcとしたs'についての式であることに注意!!
double twoπ = 2.0 * Math.PI;
double ωc = PrewarpωtoΩ(mMatchFreq * twoπ);
double k = (1.0 / ωc) * (2.0 * mSampleFreq);
var n0 = ps.N(0);
var n1k = WWComplex.Mul(ps.N(1), k);
var d0 = ps.D(0);
var d1k = WWComplex.Mul(ps.D(1), k);
var pz = new FirstOrderComplexRationalPolynomial(
WWComplex.Sub(n0, n1k), WWComplex.Add(n0, n1k),
WWComplex.Sub(d0, d1k), WWComplex.Add(d0, d1k));
return(pz);
}
public WWComplex[] ConvolutionBruteForce(WWComplex[] f, WWComplex[] g)
{
var r = new WWComplex[f.Length + g.Length - 1];
Parallel.For(0, r.Length, i => {
WWComplex v = WWComplex.Zero();
for (int j = 0; j < f.Length; ++j)
{
int fpos = f.Length - j - 1;
int gpos = i + j - (f.Length - 1);
v = WWComplex.Add(v, WWComplex.Mul(Get(f, fpos), Get(g, gpos)));
}
r[i] = v;
});
return(r);
}
/// <summary>
/// 伝達関数Hを逆ラプラス変換して時刻tのインパルス応答 h(t)を戻す。
/// </summary>
private double InverseLaplaceTransformValue(List <FirstOrderComplexRationalPolynomial> Hf,
List <FirstOrderComplexRationalPolynomial> Hi, double t)
{
if (t < 0)
{
return(0);
}
// 共役複素数のペアを作って足す。
var rvf = WWComplex.Zero();
if ((Hf.Count & 1) == 1)
{
rvf = WWComplex.Add(rvf, InverseLaplaceTransformOne(Hf[Hf.Count / 2], t));
}
if (2 <= Hf.Count)
{
for (int i = 0; i < Hf.Count / 2; ++i)
{
var p0 = Hf[i];
var p1 = Hf[Hf.Count - i - 1];
var v0 = InverseLaplaceTransformOne(p0, t);
var v1 = InverseLaplaceTransformOne(p1, t);
var v = WWComplex.Add(v0, v1);
//Console.WriteLine("{0} {1}", i, v);
rvf = WWComplex.Add(rvf, v);
}
}
var rvi = WWComplex.Zero();
foreach (var p in Hi)
{
var v = InverseLaplaceTransformOne(p, t);
rvi = WWComplex.Add(rvi, v);
}
return(WWComplex.Add(rvf, rvi).real);
}
public override WWUtil.LargeArray <double> FilterDo(WWUtil.LargeArray <double> inPcmLA)
{
var inPcm = inPcmLA.ToArray();
var pcmF = mFFTfwd.Process(inPcm);
if (pcmF.Length == 0)
{
return(new WWUtil.LargeArray <double>(0));
}
int idx20Hz = (int)(20.0 * mFftLength / mPcmFormat.SampleRate);
int idx40Hz = (int)(40.0 * mFftLength / mPcmFormat.SampleRate);
for (int i = 0; i < idx40Hz - idx20Hz; ++i)
{
// 正の周波数
{
var v = pcmF[i + idx40Hz];
v = WWComplex.Mul(v, Gain);
pcmF[i + idx20Hz] = WWComplex.Add(pcmF[i + idx20Hz], v);
}
// 負の周波数
{
var v = pcmF[mFftLength - (i + idx40Hz)];
v = WWComplex.Mul(v, Gain);
pcmF[mFftLength - (i + idx20Hz)] = WWComplex.Add(pcmF[mFftLength - (i + idx20Hz)], v);
}
}
var r = new WWUtil.LargeArray <double>(mFFTinv.Process(pcmF));
// 進捗表示。
mProcessedSamples += inPcm.Length;
ProgressReport((double)mProcessedSamples / mTotalSamples);
return(r);
}
/// <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));
}
}
}
}
/// <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));
}