diff options
Diffstat (limited to 'macros/invfreq.sci')
| -rw-r--r-- | macros/invfreq.sci | 339 |
1 files changed, 300 insertions, 39 deletions
diff --git a/macros/invfreq.sci b/macros/invfreq.sci index a720115..0fe4c81 100644 --- a/macros/invfreq.sci +++ b/macros/invfreq.sci @@ -1,43 +1,304 @@ -function [B,A] = invfreq(H,F,nB,nA,W,iter,tol, plane) -// Calculates inverse frequency vectors -// -// Calling Sequence -//[B,A] = invfreq(H,F,nB,nA) -//[B,A] = invfreq(H,F,nB,nA,W) -//[B,A] = invfreq(H,F,nB,nA,W,[],[],plane) -//[B,A] = invfreq(H,F,nB,nA,W,iter,tol,plane) -// -// Parameters -// H: desired complex frequency response,It is assumed that A and B are real polynomials, hence H is one-sided. -// F: vector of frequency samples in radians -// nA: order of denominator polynomial A -// nB: order of numerator polynomial B -// -// Description -//Fit filter B(z)/A(z) or B(s)/A(s) to complex frequency response at frequency points F. A and B are real polynomial coefficients of order nA and nB respectively. Optionally, the fit-errors can be weighted vs frequency according to the weights W. Also, the transform plane can be specified as either 's' for continuous time or 'z' for discrete time. 'z' is chosen by default. Eventually, Steiglitz-McBride iterations will be specified by iter and tol. -// -// Examples -// [B,A] = butter(12,1/4); -// [H,w] = freqz(B,A,128); -// [Bh,Ah] = invfreq(H,F,4,4); -// Hh = freqz(Bh,Ah); -// disp(sprintf('||frequency response error|| = %f',norm(H-Hh))); -// -funcprot(0); -lhs= argn(1); -rhs= argn(2); -if(rhs < 4 | rhs > 8 | rhs == 6 | rhs == 7 ) -error("Wrong number of input arguments"); -end +// Copyright (C) 2018 - IIT Bombay - FOSSEE +// This file must be used under the terms of the CeCILL. +// This source file is licensed as described in the file COPYING, which +// you should have received as part of this distribution. The terms +// are also available at +// http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt +// Original Source : https://octave.sourceforge.io/ +// Modifieded by: Abinash Singh Under FOSSEE Internship +// Last Modified on : 3 Feb 2024 +// Organization: FOSSEE, IIT Bombay +// Email: toolbox@scilab.in +/* + : [B,A] = invfreq(H,F,nB,nA,W) + : [B,A] = invfreq(H,F,nB,nA,W,[],[],plane) + : [B,A] = invfreq(H,F,nB,nA,W,iter,tol,plane) + + Fit filter B(z)/A(z) or B(s)/A(s) to complex frequency response at frequency points F. + + A and B are real polynomial coefficients of order nA and nB respectively. Optionally, the fit-errors can be weighted vs frequency according to the weights W. Also, the transform plane can be specified as either ’s’ for continuous time or ’z’ for discrete time. ’z’ is chosen by default. Eventually, Steiglitz-McBride iterations will be specified by iter and tol. + + H: desired complex frequency response It is assumed that A and B are real polynomials, hence H is one-sided. + + F: vector of frequency samples in radians + + nA: order of denominator polynomial A + + nB: order of numerator polynomial B + + plane=’z’: F on unit circle (discrete-time spectra, z-plane design) + + plane=’s’: F on jw axis (continuous-time spectra, s-plane design) + + H(k) = spectral samples of filter frequency response at points zk, where zk=exp(sqrt(-1)*F(k)) when plane=’z’ (F(k) in [0,.5]) and zk=(sqrt(-1)*F(k)) when plane=’s’ (F(k) nonnegative) +*/ +// FIXME: implement Steiglitz-McBride iterations +// FIXME: improve numerical stability for high order filters (matlab is a bit better) +// FIXME: modify to accept more argument configurations +function [B, A, SigN] = invfreq(H, F, nB, nA, W, iter, tol, tr, plane,varargin) + + if nargin < 4 then + error("invfreq : Incorrect number of input arguments ") + end -select(rhs) - case 4 then - [B,A]= callOctave("invfreq", H,F,nB,nA); - case 5 then - [B,A]= callOctave("invfreq", H,F,nB,nA, W); - case 8 then - [B,A]= callOctave("invfreq", H,F,nB, nA,iter, tol, plane); + if ~isvector(H) && ~isscalar(H) then + error("invfreq : H is the desired frequency response , a vector expected") + end + + if ~isvector(F) && ~isscalar(F) then + error("invfreq : F is a vector of frequency samples in radians") + end + + if max(size(nB)) > 1 then zB = nB(2); nB = nB(1); else zB = 0; end + n = max(nA, nB); + m = n+1; mA = nA+1; mB = nB+1; + nF = max(size(F)); + if nargin < 5 || isempty(W) then W = ones(1, nF); end + if nargin < 6 then iter = []; end + if nargin < 7 then tol = []; end + if nargin < 8 || isempty(tr) then tr = ''; end + if nargin < 9 then plane = 'z'; end + if nargin < 10 then varargin = {}; end + if ( strcmp (plane, "s") && strcmp (plane, "z")) + error (sprintf("invfreq: invealid PLANE argument %s, expected s or z ", plane)) + end + + fname = ["invfreq", plane]; + + if (nF ~= max(size(H))) then + error ("%s: Length of H and F must be the same\n", fname) + end + + if (~ isempty (iter) || ~ isempty (tol)) then + warning (sprintf("%s: iterative algorithm not yet implemented, ", ... + "ITER and TOL arguments are ignored\n", fname)); + end + +////////////////////////////////////////////////////////////// +norm = %f ; // should we normalize freqs to avoid matrices with rank deficiency ? +method = 'LS'; // by default, use Ordinary Least Square to solve normal equations +prop = varargin; +if length(prop) > 0 then + indi = 1; + while indi < length(prop) + switch prop(indi) + case 'norm' + if indi < length(prop) && ~(type(prop(indi+1)) == 10) + norm = prop(indi+1); + indi = indi + 2; // Skip the processed element + else + norm = %t; // Default true + indi = indi + 1; + end + + case 'method' + if indi < length(prop) && type(prop(indi+1)) == 10 && strcmp(prop(indi+1), "norm") + method = prop(indi+1); + indi = indi + 2; // Skip the processed element + else + error("invfreq : incorrect/missing method argument"); + indi = indi + 1; + end + + otherwise + disp("Ignoring unknown or incomplete argument"); + indi = indi + 1; + end + end end + + +//////////////////////////////////////////////////////////////// + + + Ruu = zeros(mB, mB); Ryy = zeros(nA, nA); Ryu = zeros(nA, mB); + Pu = zeros(mB, 1); Py = zeros(nA,1); + if ~strcmp(tr,'trace') + disp(' ') + disp('Computing nonuniformly sampled, equation-error, rational filter.'); + disp(['plane = ',plane]); + disp(' ') + end + + s = sqrt(-1)*F; + switch plane + case 'z' + if max(F) > %pi || min(F) < 0 then + disp('hey, you frequency is outside the range 0 to %pi, making my own') + F = linspace(0, %pi, max(size(H))); + s = sqrt(-1)*F; + end + s = exp(-s); + case 's' + if max(F) > 1e6 && n > 5 then + if ~norm then + disp('Be careful, there are risks of generating singular matrices'); + disp('Call invfreqs as (..., norm, 1) to avoid it'); + else + Fmax = max(F); s = sqrt(-1)*F/Fmax; + end + end + end + + ////////////////////////////// + ///////////////////////////// + for k=1:nF, + Zk = (s(k).^[0:n]).'; + Hk = H(k); + aHks = Hk*conj(Hk); + Rk = (W(k)*Zk)*Zk'; + rRk = real(Rk); + Ruu = clean(Ruu + rRk(1:mB, 1:mB)); + Ryy = Ryy + aHks*rRk(2:mA, 2:mA); + Ryu = Ryu + real(Hk*Rk(2:mA, 1:mB)); + Pu = Pu + W(k)*real(conj(Hk)*Zk(1:mB)); + Py = Py + (W(k)*aHks)*real(Zk(2:mA)); + end + Rr = ones(max(size(s)), mB+nA); Zk = s; + for k = 1:min(nA, nB), + Rr(:, 1+k) = Zk; + Rr(:, mB+k) = -Zk.*H; + Zk = Zk.*s; + end + for k = 1+min(nA, nB):max(nA, nB)-1, + if k <= nB, Rr(:, 1+k) = Zk; end + if k <= nA, Rr(:, mB+k) = -Zk.*H; end + Zk = Zk.*s; + end + k = k+1; + if k <= nB then Rr(:, 1+k) = Zk; end + if k <= nA then Rr(:, mB+k) = -Zk.*H; end + + // complex to real equation system -- this ensures real solution + Rr = Rr(:, 1+zB:$); + Rr = [real(Rr); imag(Rr)]; Pr = [real(H(:)); imag(H(:))]; + // normal equations -- keep for ref + // Rn= [Ruu(1+zB:mB, 1+zB:mB), -Ryu(:, 1+zB:mB)'; -Ryu(:, 1+zB:mB), Ryy]; + // Pn= [Pu(1+zB:mB); -Py]; + //////////////////////////////////////////////// + switch method + case {'ls' 'LS'} + // avoid scaling errors with Theta = R\P; + // [Q, R] = qr([Rn Pn]); Theta = R(1:$, 1:$-1)\R(1:$, $); + [Q, R] = qr([Rr Pr]); Theta = pinv(R(1:$-1, 1:$-1)) * R(1:$-1, $); + ////////////////////////////////////////////////// + // SigN = R($, $-1); + SigN = R($, $); + case {'tls' 'TLS'} + // [U, S, V] = svd([Rn Pn]); + // SigN = S($, $-1); + // Theta = -V(1:$-1, $)/V($, $); + [U, S, V] = svd([Rr Pr], 0); + SigN = S($, $); + Theta = -V(1:$-1, $)/V($, $); + case {'mls' 'MLS' 'qr' 'QR'} + // [Q, R] = qr([Rn Pn], 0); + // solve the noised part -- DO NOT USE ECONOMY SIZE ~ + // [U, S, V] = svd(R(nA+1:$, nA+1:$)); + // SigN = S($, $-1); + // Theta = -V(1:$-1, $)/V($, $); + // unnoised part -- remove B contribution and back-substitute + // Theta = [R(1:nA, 1:nA)\(R(1:nA, $) - R(1:nA, nA+1:$-1)*Theta) + // Theta]; + // solve the noised part -- economy size OK as #rows > #columns + [Q, R] = qr([Rr Pr], 0); + eB = mB-zB; sA = eB+1; + [U, S, V] = svd(R(sA:$, sA:$)); + // noised (A) coefficients + Theta = -V(1:$-1, $)/V($, $); + // unnoised (B) part -- remove A contribution and back-substitute + Theta = [R(1:eB, 1:eB)\(R(1:eB, $) - R(1:eB, sA:$-1)*Theta) + Theta]; + SigN = S($, $); + otherwise + error(sprintf("invfreq : unknown method %s", method)); + end + + B = [zeros(zB, 1); Theta(1:mB-zB)].'; + A = [1; Theta(mB-zB+(1:nA))].'; + if ~strcmp(plane,'s') + B = B(mB:-1:1); + A = A(mA:-1:1); + if norm, // Frequencies were normalized -- unscale coefficients + Zk = Fmax.^[n:-1:0].'; + for k = nB:-1:1+zB, B(k) = B(k)/Zk(k); end + for k = nA:-1:1, A(k) = A(k)/Zk(k); end + end + end endfunction - +/* +// method - LS +test case 1 // passed + +[B,A,Sign] = invfreq(1,1,1,1,1,[],[],'','z','norm',1,'method','LS') +assert_checkequal(B,[0.6314 0.3411]) +assert_checkequal(A,[1 -0.3411]) +assert_checkequal(Sign,0) + +[B,A,Sign] = invfreq(1,1,1,1,1,[],[],'','s') +assert_checkequal(B,[0 1]) +assert_checkequal(A,[0 1]) +assert_checkequal(Sign,0) + + +test case 2 // passed +order = 6 +fc = 1/2 +n = 128 +B = [ 0.029588 0.177529 0.443823 0.591764 0.443823 0.177529 0.029588] ; +A = [ 1.0000e+00 -6.6613e-16 7.7770e-01 -2.8192e-16 1.1420e-01 -1.4472e-17 1.7509e-03]; +[H,w] = freqz(B,A,n) ; +[Bh , Ah] = invfreq(H,w,order,order); +[Hh,wh] = freqz(Bh,Ah,n); +plot(w,[abs(H), abs(Hh)]) +xlabel("Frequency (rad/sample)"); +ylabel("Magnitude"); +legend('Original','Measured'); +err = norm(H-Hh); +disp(sprintf('L2 norm of frequency response error = %f',err)); + +test case 3 // passed +// buttter worth filter of order 12 and fc=1/4 +B = [ 1.1318e-06 1.3582e-05 7.4702e-05 2.4901e-04 5.6026e-04 8.9642e-04 1.0458e-03 8.9642e-04 5.6026e-04 2.4901e-04 7.4702e-05 1.3582e-05 1.1318e-06]; +A = [ 1.0000e+00 -5.9891e+00 1.7337e+01 -3.1687e+01 4.0439e+01 -3.7776e+01 2.6390e+01 -1.3851e+01 5.4089e+00 -1.5296e+00 2.9688e-01 -3.5459e-02 1.9688e-03]; +[H,w] = freqz(B,A,128); +[Bh,Ah] = invfreq(H,w,4,4); +[Hh,wh] = freqz(Bh,Ah,128); +disp(sprintf('||frequency response error||= %f',norm(H-Hh))); + + +method TLS + +test case 1 // passed + +B = [ 1.1318e-06 1.3582e-05 7.4702e-05 2.4901e-04 5.6026e-04 8.9642e-04 1.0458e-03 8.9642e-04 5.6026e-04 2.4901e-04 7.4702e-05 1.3582e-05 1.1318e-06]; +A = [ 1.0000e+00 -5.9891e+00 1.7337e+01 -3.1687e+01 4.0439e+01 -3.7776e+01 2.6390e+01 -1.3851e+01 5.4089e+00 -1.5296e+00 2.9688e-01 -3.5459e-02 1.9688e-03]; +[H,w] = freqz(B,A,128); +[Bh,Ah] = invfreq(H,w,4,4,[],[],[],'','z','norm',1,'method','TLS'); +[Hh,wh] = freqz(Bh,Ah,128); +disp(sprintf('||frequency response error||= %f',norm(H-Hh))); + + + + +method MLS - passed + + +// elliptic filter with ellip (5, 1, 90, [.1 .2]) +n = 128 +B = [ 1.3214e-04 -6.6404e-04 1.4928e-03 -1.9628e-03 1.4428e-03 0 -1.4428e-03 1.9628e-03 -1.4928e-03 6.6404e-04 -1.3214e-04] ; +A = [ 1.0000 -8.6483 34.6032 -84.2155 137.9276 -158.7598 130.0425 -74.8636 29.0044 -6.8359 0.7456]; +[H,w] = freqz(B,A,n) ; + +[Bh,Ah] = invfreq(H,w,4,4,[],[],[],'','z','norm',1,'method','MLS'); +[Hh,wh] = freqz(Bh,Ah,n); +plot(w,[abs(H), abs(Hh)]) +xlabel("Frequency (rad/sample)"); +ylabel("Magnitude"); +legend('Original','Measured'); +err = norm(H-Hh); +disp(sprintf('L2 norm of frequency response error = %f',err)); + + +*/ |