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>
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
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
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/>.
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)
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.
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)
43 %% [B,A] = butter(12,1/4);
44 %% [H,w] = freqz(B,A,128);
45 %% [Bh,Ah] = invfreq(H,F,4,4);
47 %% disp(sprintf('||frequency response error|| = %f',norm(H-Hh)));
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,
53 %% http://ccrma.stanford.edu/~jos/filters/FFT_Based_Equation_Error_Method.html
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
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
62 m = n+1; mA = nA+1; mB = nB+1;
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
75 [reg, prop ] = parseparams(varargin);
76 %# should we normalise freqs to avoid matrices with rank deficiency ?
78 %# by default, use Ordinary Least Square to solve normal equations
81 indi = 1; while indi <= length(prop)
84 if indi < length(prop) && ~ischar(prop{indi+1}),
85 norm = logical(prop{indi+1});
86 prop(indi:indi+1) = [];
89 norm = true; prop(indi) = [];
93 if indi < length(prop) && ischar(prop{indi+1}),
94 method = prop{indi+1};
95 prop(indi:indi+1) = [];
98 error('invfreq.m: incorrect/missing method argument');
100 otherwise %# FIXME: just skip it for now
101 disp(sprintf("Ignoring unkown argument %s", varargin{indi}));
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')
111 disp('Computing nonuniformly sampled, equation-error, rational filter.');
112 disp(['plane = ',plane]);
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));
126 if max(F) > 1e6 && n > 5,
128 disp('Be carefull, there are risks of generating singular matrices');
129 disp('Call invfreqs as (..., "norm", true) to avoid it');
131 Fmax = max(F); s = sqrt(-1)*F/Fmax;
137 Zk = (s(k).^[0:n]).';
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));
148 Rr = ones(length(s), mB+nA); Zk = s;
149 for k = 1:min(nA, nB),
151 Rr(:, mB+k) = -Zk.*H;
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
160 if k <= nB, Rr(:, 1+k) = Zk; end
161 if k <= nA, Rr(:, mB+k) = -Zk.*H; end
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];
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);
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);
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)
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)
204 error("invfreq: unknown method %s", method);
207 B = [zeros(zB, 1); Theta(1:mB-zB)].';
208 A = [1; Theta(mB-zB+(1:nA))].';
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
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');
234 %! disp(sprintf('L2 norm of frequency response error = %f',err));