View Raw SPL
/*****************************************************************************
* *
* INVFREQ.SPL Copyright (C) 2009 DSP Development Corporation *
* All Rights Reserved *
* *
* Author: Randy Race *
* *
* Synopsis: Computes an analog or digital filter *
* *
* Revisions: 13 Aug 2009 RRR Creation *
* *
*****************************************************************************/
#include
/*
* computes the discrete or analog filter coefficients from a complex
* frequency response
*
* see INVFREQS and INVFREQZ for details
*/
invfreq(h, w, complex, nb, na, wf, maxiter, tol, pf, plane, iter)
{
local a, b, e, g, nk, rw, cw, rwf, cwf, rg, cg, nm, OM, T;
local Dva, Dvb, D, R, Vd, th, kom, indb, indg, inda;
local Gc, Vcap, indb1, indg1, inda1, gndir, l, st;
local D31, D32, D3, ll, k, V1, t1, aT, bT;
/* parse args */
(h, w, complex, nb, na, wf, maxiter, tol, pf, plane, iter) = invfreq_parse_args(h, w, complex, nb, na, wf, maxiter, tol, pf, plane, iter);
/* real or complex frequencies */
realFlag = strcmp(complex, "complex") != 0;
// constrain numerator to nk zeros
nk = 0;
nb = nb + nk + 1;
wf = sqrt(wf);
g = h;
setdeltax(g, 1.0);
if (length(g) != length(w)) error('invfreq: h and w must be the same length');
if (length(wf) != length(w)) error('invfreq: wf and w must be the same length');
/* complex frequency check */
if (any(w[..] < 0) && realFlag)
{
warnStr = strescape
(
"w contains negative frequencies that will be aliased to \n
positive values. Use the 'complex' flag to design a complex filter."
);
message(warnStr);
}
/* ensure orientation */
(rw, cw) = size(w);
if (rw > cw)
{
w = w~^;
}
(rg, cg) = size(g);
if (cg > rg)
{
g = g';
}
(rwf, cwf) = size(wf);
if (cwf > rwf)
{
wf = wf~^;
}
nm = max(na + 1, nb + nk);
if (plane == 's')
{
/* continuous domain */
indb = nb.. - 1..1;
indg = (na + 1).. - 1..1;
inda = na.. - 1..1;
OM = ones(1, numcols(w)) + 0i;
if (nm > 1)
{
kom = 0..(nm - 1);
OM = (i * w) ^ kom;
}
}
else
{
/* discrete domain */
indb = nk + 1..nk + nb;
indg = 1..na + 1;
inda = 2..na + 1;
T = 1;
OM = exp(-i * (0..nm) *^ w * T);
}
/*
* least square estimation step
*/
Dva = (OM[inda, ..]') * (g * ones(1, na));
Dvb = -(OM[indb, ..]');
D = ravel(Dva, Dvb) * (wf * ones(1, na + nb));
R = D~^ *^ D;
if (plane == 'z')
{
Vd = D~^ *^ (-g * wf);
}
else
{
Vd = D~^ *^ ((-g * OM[na+1, ..]') * wf);
}
if (realFlag)
{
R = real(R);
Vd = real(Vd);
}
th = R ^ Vd;
a = {1, th[1..na]};
b = {zeros(nk, 1), th[(na + 1)..(na + nb)]};
if (iter)
{
/* iterative minimization */
if (isempty(maxiter)) maxiter = 30;
if (isempty(tol)) tol = 0.01;
if (plane == 'z')
{
indb1 = 1..length(b);
indg1 = 1..length(a);
}
else
{
indb1 = indb;
indg1 = indg;
}
/* Stabilize the denominator */
a = ppolystab(a, realFlag, plane);
/* initial estimate */
aT = a';
bT = b';
GC = (transpose(bT *^ OM[indb1, ..]) / (transpose(aT *^ OM[indg1, ..])));
e = (GC - g) * wf;
Vcap = e~^ *^ e;
t = {a[2..na+1], b[nk+1..nk+nb]};
/*
* minimization loop
*/
gndir = 2 * tol + 1;
l = 0;
st = 0;
while (all({norm({gndir}) > tol, l < maxiter, st != 1}))
{
l++;
/* gradient */
D31 = (transpose(OM[inda, ..])) * (-GC / (transpose(aT *^ OM[indg, ..])) *^ ones(1, na));
D32 = (transpose(OM[indb, ..])) / (transpose(aT *^ OM[indg, ..]) *^ ones(1, nb));
D3 = ravel(D31, D32) * (wf * ones(1, na + nb));
/* Gauss-Newton search direction */
e = (GC - g) * wf;
R = D3~^ *^ D3;
Vd = D3~^ *^ e;
if (realFlag)
{
R = real(R);
Vd = real(Vd);
}
gndir = R ^ Vd;
/* search along Gauss-Newton direction */
ll = 0;
k = 1;
V1 = Vcap + 1;
while (all( {V1 > Vcap, ll < 20}))
{
t1 = t - k * gndir;
if (ll == 19)
{
t1 = t;
}
a = {1, t1[1..na]};
b = {zeros(1, nk), t1[na+1..na+nb]};
/* stabilize the denominator */
a = ppolystab(a, realFlag, plane);
t1[1..na] = a[2..na+1];
aT = a';
bT = b';
GC = (transpose(bT *^ OM[indb, ..])) / (transpose(aT *^ OM[indg, ..]));
V1 = ((GC - g) * wf)~^ *^ ((GC - g) * wf);
k /= 2;
ll++;
if (ll == 10)
{
gndir = Vd /^ norm(R) * length(R);
k = 1;
}
if (ll == 20)
{
st = 1;
}
}
t = t1;
Vcap = V1;
}
}
setcomment(b, "Num");
setcomment(a, "Den");
if (outargc > 1)
{
return(b, a);
}
else
{
return(ravel(b, a));
}
}
/* stabilize a denominator polynomial based on z or s plane */
ppolystab(a, realFlag, plane)
{
local b;
if (argc < 3)
{
if (argc < 2)
{
if (argc < 1) error("ppolystab - input required");
realFlag = 1;
}
plane = 'z';
}
if (plane == 'z')
{
b = polystab(a);
}
else
{
b = apolystab(a, realFlag);
}
return(b);
}
/* stabilize a denominator polynomial */
apolystab(a, realFlag)
{
local v, vind;
if (length(a) > 0)
{
v = roots(a);
vind = find(real(v) > 0);
v[vind] = -v[vind];
a = poly(v);
if (realFlag)
{
a = real(a);
}
}
return(a);
}
/* parse input arguments to invfreq */
invfreq_parse_args(g, w, complex, nb, na, wf, maxiter, tol, pf, plane, iter)
{
switch (argc)
{
case 0:
error("invfreq - input gain a frequencies required");
break;
case 1:
w = xvals(g);
case 2:
complex = "";
case 3:
nb = {};
case 4:
na = {};
case 5:
wf = {};
case 6:
maxiter = {};
case 7:
tol = {};
case 8:
pf = {};
case 9:
plane = {};
case 10:
iter = {};
case 11:
break;
default:
error("too many arguments for invfreq");
break;
}
/* check complex flag */
if (not(isstring(complex)))
{
iter = plane;
plane = pf;
pf = tol;
tol = maxiter;
maxiter = wf;
wf = na;
na = nb;
nb = complex;
complex = "";
}
if (isempty(nb))
{
nb = 1;
}
if (isempty(na))
{
na = 1;
}
if (isempty(wf))
{
wf = ones(length(w), 1);
}
if (isempty(plane))
{
plane = "z";
}
if (isempty(iter))
{
iter = 0;
}
return(g, w, complex, nb, na, wf, maxiter, tol, pf, plane, iter);
}