private WWComplex TransferFunction(WWComplex z)
{
var zR = WWComplex.Reciprocal(z);
var zRecip = new WWComplex[ORDER_PLUS_1];
zRecip[0] = WWComplex.Unity();
for (int i = 1; i < zRecip.Length; ++i)
{
zRecip[i] = WWComplex.Mul(zRecip[i - 1], zR);
}
var denom = WWComplex.Zero();
for (int i = 0; i < zRecip.Length; ++i)
{
denom = WWComplex.Add(denom, WWComplex.Mul(new WWComplex(mDenom[i], 0), zRecip[i]));
}
var numer = WWComplex.Zero();
for (int i = 0; i < zRecip.Length; ++i)
{
numer = WWComplex.Add(numer, WWComplex.Mul(new WWComplex(mNumer[i], 0), zRecip[i]));
}
var h = WWComplex.Div(numer, denom);
// 孤立特異点や極で起きる異常を適当に除去する
if (double.IsNaN(h.Magnitude()))
{
return(new WWComplex(0.0f, 0.0f));
}
return(h);
}
/// <summary>
/// アナログフィルターの零や極の座標をバイリニア変換する。
/// Discrete-time signal processing 3rd ed. pp534 eq7.21
/// 三谷政昭, ディジタル・フィルタ理論&設計入門 pp.193
/// </summary>
/// <param name="s">s平面上の座標s'。アナログフィルターのs'は、s' == s / ωcとしたs'についての式になっている</param>
/// <param name="ωc">マッチ周波数(rad)。アナログフィルターとIIRフィルターの特性が一致する周波数。</param>
/// <returns>IIRフィルターのZ平面上の座標</returns>
public WWComplex StoZ(WWComplex s, double ωc)
{
WWComplex s2 = s.Scale(ωc * Td / 2.0);
return(WWComplex.Div(
new WWComplex(1.0 + s2.real, s2.imaginary),
new WWComplex(1.0 - s2.real, -s2.imaginary)
));
}
/// <summary>
/// 伝達関数の地点sの値を計算する。
/// </summary>
private WWComplex TransferFunctionValue(WWComplex s)
{
var numer = new WWComplex(mNumeratorConstant, 0);
for (int i = 0; i < mZeroList.Count; ++i)
{
var b = mZeroList[i];
numer = WWComplex.Mul(numer, WWComplex.Sub(s, b));
}
var denom = WWComplex.Unity();
for (int i = 0; i < mPoleList.Count; ++i)
{
var a = mPoleList[i];
denom = WWComplex.Mul(denom, WWComplex.Sub(s, a));
}
return(WWComplex.Div(numer, denom));
}
/// <summary>
/// Calc concrete value of Transfer function H(s)
/// </summary>
/// <param name="s">coordinate on s plane</param>
/// <returns>transfer function value</returns>
public WWComplex H(WWComplex s)
{
WWComplex numerator = new WWComplex(TransferFunctionConstant(), 0);
for (int i = 0; i < NumOfZeroes(); ++i)
{
var b = ZeroNth(i);
numerator = WWComplex.Mul(numerator, WWComplex.Sub(s, b));
}
WWComplex denominator = WWComplex.Unity();
for (int i = 0; i < mN; ++i)
{
WWComplex a = PoleNth(i);
denominator = WWComplex.Mul(denominator, WWComplex.Sub(s, a));
}
return(WWComplex.Div(numerator, denominator));
}
/// <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);
}
}
/// <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));
}
}
}
}