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