octave_packages/signal-1.1.3/invfreq.m

   1 %% Copyright (C) 1986, 2000, 2003 Julius O. Smith III <jos@ccrma.stanford.edu>
   2 %% Copyright (C) 2007 Rolf Schirmacher <Rolf.Schirmacher@MuellerBBM.de>
   3 %% Copyright (C) 2003 Andrew Fitting
   4 %% Copyright (C) 2010 Pascal Dupuis <Pascal.Dupuis@uclouvain.be>
   5 %%
   6 %% This program is free software; you can redistribute it and/or modify it under
   7 %% the terms of the GNU General Public License as published by the Free Software
   8 %% Foundation; either version 3 of the License, or (at your option) any later
   9 %% version.
  10 %%
  11 %% This program is distributed in the hope that it will be useful, but WITHOUT
  12 %% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  13 %% FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
  14 %% details.
  15 %%
  16 %% You should have received a copy of the GNU General Public License along with
  17 %% this program; if not, see <http://www.gnu.org/licenses/>.
  18
  19 %% usage: [B,A] = invfreq(H,F,nB,nA)
  20 %%        [B,A] = invfreq(H,F,nB,nA,W)
  21 %%        [B,A] = invfreq(H,F,nB,nA,W,[],[],plane)
  22 %%        [B,A] = invfreq(H,F,nB,nA,W,iter,tol,plane)
  23 %%
  24 %% Fit filter B(z)/A(z) or B(s)/A(s) to complex frequency response at
  25 %% frequency points F. A and B are real polynomial coefficients of order
  26 %% nA and nB respectively.  Optionally, the fit-errors can be weighted vs
  27 %% frequency according to the weights W. Also, the transform plane can be
  28 %% specified as either 's' for continuous time or 'z' for discrete time. 'z'
  29 %% is chosen by default.  Eventually, Steiglitz-McBride iterations will be
  30 %% specified by iter and tol.
  31 %%
  32 %% H: desired complex frequency response
  33 %%     It is assumed that A and B are real polynomials, hence H is one-sided.
  34 %% F: vector of frequency samples in radians
  35 %% nA: order of denominator polynomial A
  36 %% nB: order of numerator polynomial B
  37 %% plane='z': F on unit circle (discrete-time spectra, z-plane design)
  38 %% plane='s': F on jw axis     (continuous-time spectra, s-plane design)
  39 %% H(k) = spectral samples of filter frequency response at points zk,
  40 %%  where zk=exp(sqrt(-1)*F(k)) when plane='z' (F(k) in [0,.5])
  41 %%     and zk=(sqrt(-1)*F(k)) when plane='s' (F(k) nonnegative)
  42 %% Example:
  43 %%     [B,A] = butter(12,1/4);
  44 %%     [H,w] = freqz(B,A,128);
  45 %%     [Bh,Ah] = invfreq(H,F,4,4);
  46 %%     Hh = freqz(Bh,Ah);
  47 %%     disp(sprintf('||frequency response error|| = %f',norm(H-Hh)));
  48 %%
  49 %% References: J. O. Smith, "Techniques for Digital Filter Design and System
  50 %%      Identification with Application to the Violin, Ph.D. Dissertation,
  51 %%      Elec. Eng. Dept., Stanford University, June 1983, page 50; or,
  52 %%
  53 %% http://ccrma.stanford.edu/~jos/filters/FFT_Based_Equation_Error_Method.html
  54
  55 %% TODO: implement Steiglitz-McBride iterations
  56 %% TODO: improve numerical stability for high order filters (matlab is a bit better)
  57 %% TODO: modify to accept more argument configurations
  58
  59 function [B, A, SigN] = invfreq(H, F, nB, nA, W, iter, tol, tr, plane, varargin)
  60   if length(nB) > 1, zB = nB(2); nB = nB(1); else zB = 0; end
  61   n = max(nA, nB);
  62   m = n+1; mA = nA+1; mB = nB+1;
  63   nF = length(F);
  64   if nF ~= length(H), disp('invfreqz: length of H and F must be the same'); end;
  65   if nargin < 5 || isempty(W), W = ones(1, nF); end;
  66   if nargin < 6, iter = []; end
  67   if nargin < 7  tol = []; end
  68   if nargin < 8 || isempty(tr), tr = ''; end
  69   if nargin < 9, plane = 'z'; end
  70   if nargin < 10, varargin = {}; end
  71   if iter~=[], disp('no implementation for iter yet'),end
  72   if tol ~=[], disp('no implementation for tol yet'),end
  73   if (plane ~= 'z' && plane ~= 's'), disp('invfreqz: Error in plane argument'), end
  74
  75   [reg, prop ] = parseparams(varargin);
  76   %# should we normalise freqs to avoid matrices with rank deficiency ?
  77   norm = false;
  78   %# by default, use Ordinary Least Square to solve normal equations
  79   method = 'LS';
  80   if length(prop) > 0
  81     indi = 1; while indi <= length(prop)
  82       switch prop{indi}
  83         case 'norm'
  84           if indi < length(prop) && ~ischar(prop{indi+1}),
  85             norm = logical(prop{indi+1});
  86             prop(indi:indi+1) = [];
  87             continue
  88           else
  89             norm = true; prop(indi) = [];
  90             continue
  91            end
  92          case 'method'
  93            if indi < length(prop) && ischar(prop{indi+1}),
  94              method = prop{indi+1};
  95              prop(indi:indi+1) = [];
  96             continue
  97            else
  98              error('invfreq.m: incorrect/missing method argument');
  99            end
 100          otherwise %# FIXME: just skip it for now
 101            disp(sprintf("Ignoring unkown argument %s", varargin{indi}));
 102            indi = indi + 1;
 103       end
 104     end
 105   end
 106
 107   Ruu = zeros(mB, mB); Ryy = zeros(nA, nA); Ryu = zeros(nA, mB);
 108   Pu = zeros(mB, 1);   Py = zeros(nA,1);
 109   if strcmp(tr,'trace')
 110       disp(' ')
 111       disp('Computing nonuniformly sampled, equation-error, rational filter.');
 112       disp(['plane = ',plane]);
 113       disp(' ')
 114   end
 115
 116   s = sqrt(-1)*F;
 117   switch plane
 118     case 'z'
 119       if max(F) > pi || min(F) < 0
 120       disp('hey, you frequency is outside the range 0 to pi, making my own')
 121         F = linspace(0, pi, length(H));
 122       s = sqrt(-1)*F;
 123     end
 124     s = exp(-s);
 125     case 's'
 126       if max(F) > 1e6 && n > 5,
 127         if ~norm,
 128           disp('Be carefull, there are risks of generating singular matrices');
 129           disp('Call invfreqs as (..., "norm", true) to avoid it');
 130         else
 131           Fmax = max(F); s = sqrt(-1)*F/Fmax;
 132         end
 133       end
 134   end
 135
 136   for k=1:nF,
 137     Zk = (s(k).^[0:n]).';
 138     Hk = H(k);
 139     aHks = Hk*conj(Hk);
 140     Rk = (W(k)*Zk)*Zk';
 141     rRk = real(Rk);
 142     Ruu = Ruu + rRk(1:mB, 1:mB);
 143     Ryy = Ryy + aHks*rRk(2:mA, 2:mA);
 144     Ryu = Ryu + real(Hk*Rk(2:mA, 1:mB));
 145     Pu = Pu + W(k)*real(conj(Hk)*Zk(1:mB));
 146     Py = Py + (W(k)*aHks)*real(Zk(2:mA));
 147   end;
 148   Rr = ones(length(s), mB+nA); Zk = s;
 149   for k = 1:min(nA, nB),
 150     Rr(:, 1+k) = Zk;
 151     Rr(:, mB+k) = -Zk.*H;
 152     Zk = Zk.*s;
 153   end
 154   for k = 1+min(nA, nB):max(nA, nB)-1,
 155     if k <= nB, Rr(:, 1+k) = Zk; end
 156     if k <= nA, Rr(:, mB+k) = -Zk.*H; end
 157     Zk = Zk.*s;
 158   end
 159   k = k+1;
 160   if k <= nB, Rr(:, 1+k) = Zk; end
 161   if k <= nA, Rr(:, mB+k) = -Zk.*H; end
 162
 163   %# complex to real equation system -- this ensures real solution
 164   Rr = Rr(:, 1+zB:end);
 165   Rr = [real(Rr); imag(Rr)]; Pr = [real(H(:)); imag(H(:))];
 166   %# normal equations -- keep for ref
 167   %# Rn= [Ruu(1+zB:mB, 1+zB:mB), -Ryu(:, 1+zB:mB)';  -Ryu(:, 1+zB:mB), Ryy];
 168   %# Pn= [Pu(1+zB:mB); -Py];
 169
 170   switch method
 171     case {'ls' 'LS'}
 172       %# avoid scaling errors with Theta = R\P;
 173       %# [Q, R] = qr([Rn Pn]); Theta = R(1:end, 1:end-1)\R(1:end, end);
 174       [Q, R] = qr([Rr Pr], 0); Theta = R(1:end-1, 1:end-1)\R(1:end-1, end);
 175       %# SigN = R(end, end-1);
 176       SigN = R(end, end);
 177     case {'tls' 'TLS'}
 178       % [U, S, V] = svd([Rn Pn]);
 179       % SigN = S(end, end-1);
 180       % Theta =  -V(1:end-1, end)/V(end, end);
 181       [U, S, V] = svd([Rr Pr], 0);
 182       SigN = S(end, end);
 183       Theta =  -V(1:end-1, end)/V(end, end);
 184     case {'mls' 'MLS' 'qr' 'QR'}
 185       % [Q, R] = qr([Rn Pn], 0);
 186       %# solve the noised part -- DO NOT USE ECONOMY SIZE !
 187       % [U, S, V] = svd(R(nA+1:end, nA+1:end));
 188       % SigN = S(end, end-1);
 189       % Theta = -V(1:end-1, end)/V(end, end);
 190       %# unnoised part -- remove B contribution and back-substitute
 191       % Theta = [R(1:nA, 1:nA)\(R(1:nA, end) - R(1:nA, nA+1:end-1)*Theta)
 192       %         Theta];
 193       %# solve the noised part -- economy size OK as #rows > #columns
 194       [Q, R] = qr([Rr Pr], 0);
 195       eB = mB-zB; sA = eB+1;
 196       [U, S, V] = svd(R(sA:end, sA:end));
 197       %# noised (A) coefficients
 198       Theta = -V(1:end-1, end)/V(end, end);
 199       %# unnoised (B) part -- remove A contribution and back-substitute
 200       Theta = [R(1:eB, 1:eB)\(R(1:eB, end) - R(1:eB, sA:end-1)*Theta)
 201               Theta];
 202       SigN = S(end, end);
 203     otherwise
 204       error("invfreq: unknown method %s", method);
 205   end
 206
 207   B = [zeros(zB, 1); Theta(1:mB-zB)].';
 208   A = [1; Theta(mB-zB+(1:nA))].';
 209
 210   if strcmp(plane,'s')
 211     B = B(mB:-1:1);
 212     A = A(mA:-1:1);
 213     if norm, %# Frequencies were normalised -- unscale coefficients
 214       Zk = Fmax.^[n:-1:0].';
 215       for k = nB:-1:1+zB, B(k) = B(k)/Zk(k); end
 216       for k = nA:-1:1, A(k) = A(k)/Zk(k); end
 217     end
 218   end
 219 endfunction
 220
 221 %!demo
 222 %! order = 6; % order of test filter
 223 %! fc = 1/2;   % sampling rate / 4
 224 %! n = 128;    % frequency grid size
 225 %! [B, A] = butter(order,fc);
 226 %! [H, w] = freqz(B,A,n);
 227 %! [Bh, Ah] = invfreq(H,w,order,order);
 228 %! [Hh, wh] = freqz(Bh,Ah,n);
 229 %! xlabel("Frequency (rad/sample)");
 230 %! ylabel("Magnitude");
 231 %! plot(w,[abs(H), abs(Hh)])
 232 %! legend('Original','Measured');
 233 %! err = norm(H-Hh);
 234 %! disp(sprintf('L2 norm of frequency response error = %f',err));