## Copyright (C) 2005 Julius O. Smith III ## Copyright (C) 2021-2022 Charles Praplan ## ## This program is free software: you can redistribute it and/or modify ## it under the terms of the GNU General Public License as published by ## the Free Software Foundation, either version 3 of the License, or ## (at your option) any later version. ## ## This program is distributed in the hope that it will be useful, ## but WITHOUT ANY WARRANTY; without even the implied warranty of ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ## GNU General Public License for more details. ## ## You should have received a copy of the GNU General Public License ## along with this program; see the file COPYING. If not, see ## . ## -*- texinfo -*- ## @deftypefn {Function File} {[@var{sos}, @var{g}] =} zp2sos (@var{z}) ## @deftypefnx {Function File} {[@var{sos}, @var{g}] =} zp2sos (@var{z}, @var{p}) ## @deftypefnx {Function File} {[@var{sos}, @var{g}] =} zp2sos (@var{z}, @var{p}, @var{k}) ## @deftypefnx {Function File} {@var{sos} =} zp2sos (@dots{}) ## Convert filter poles and zeros to second-order sections. ## ## INPUTS: ## @itemize ## @item ## @var{z} = column-vector containing the filter zeros ## @item ## @var{p} = column-vector containing the filter poles ## @item ## @var{k} = overall filter gain factor. If not given the gain is assumed to be 1. ## @end itemize ## ## RETURNED: ## @itemize ## @item ## @var{sos} = matrix of series second-order sections, one per row: ## @example ## @var{sos} = [@var{B1}.' @var{A1}.'; ...; @var{BN}.' @var{AN}.'] ## @end example ## where ## @code{@var{B1}.' = [b0 b1 b2] and @var{A1}.' = [a0 a1 a2]} for ## section 1, etc. ## See @code{filter} for documentation of the second-order direct-form filter ## coefficients @var{B}i and %@var{A}i, i=1:N. ## ## @item ## @var{g} is the overall gain factor that effectively scales ## any one of the @var{B}i vectors. ## @end itemize ## ## If called with only one output argument, the overall filter gain is ## applied to the first second-order section in the matrix @var{sos}. ## ## EXAMPLE: ## @example ## [z, p, k] = tf2zp ([1 0 0 0 0 1], [1 0 0 0 0 .9]); ## [sos, g] = zp2sos (z, p, k) ## ## sos = ## 1.0000 0.6180 1.0000 1.0000 0.6051 0.9587 ## 1.0000 -1.6180 1.0000 1.0000 -1.5843 0.9587 ## 1.0000 1.0000 0 1.0000 0.9791 0 ## ## g = ## 1 ## @end example ## ## @seealso{sos2zp, sos2tf, tf2sos, zp2tf, tf2zp} ## @end deftypefn function [SOS,G] = zp2sos(z,p,k,DoNotCombineReal) if nargin<3, k=1; endif if nargin<2, p=[]; endif DoNotCombineReal = 0; [zc, zr] = cplxreal (z(:)); [pc, pr] = cplxreal (p(:)); nzc = length (zc); npc = length (pc); nzr = length (zr); npr = length (pr); if DoNotCombineReal # Handling complex conjugate poles for count = 1:npc SOS(count, 4:6) = [1, -2 * real(pc(count)), abs(pc(count))^2]; endfor # Handling real poles for count = 1:npr SOS(count + npc, 4:6) = [0, 1, -pr(count)]; endfor # Handling complex conjugate zeros for count = 1:nzc SOS(count, 1:3) = [1, -2 * real(zc(count)), abs(zc(count))^2]; endfor # Handling real zeros for count = 1:nzr SOS(count+nzc, 1:3) = [0, 1, -zr(count)]; endfor # Completing SOS if needed (sections without pole or zero) if npc + npr > nzc + nzr for count = nzc + nzr + 1 : npc + npr % sections without zero SOS(count, 1:3) = [0, 0, 1]; end else for count = npc + npr + 1 : nzc + nzr % sections without pole SOS(count, 4:6) = [0, 0, 1]; endfor endif else # Handling complex conjugate poles for count = 1:npc SOS(count, 4:6) = [1, -2 * real(pc(count)), abs(pc(count))^2]; endfor # Handling pair of real poles for count = 1:floor(npr/2) SOS(count+npc, 4:6) = [1, - pr(2 * count - 1) - pr(2 * count), pr(2 * count - 1) * pr(2 * count)]; endfor # Handling last real pole (if any) if mod (npr,2) == 1 SOS(npc + floor (npr / 2) + 1, 4:6)= [0, 1, -pr(end)]; endif # Handling complex conjugate zeros for count = 1:nzc SOS(count, 1:3)= [1, -2 * real(zc(count)), abs(zc(count))^2]; endfor # Handling pair of real zeros for count = 1:floor(nzr / 2) SOS(count+nzc, 1:3)= [1, - zr(2 * count - 1) - zr(2 * count), zr(2 * count - 1) * zr(2 * count)]; endfor # Handling last real zero (if any) if mod (nzr, 2) == 1 SOS(nzc + floor (nzr / 2) + 1, 1:3) = [0, 1, -zr(end)]; endif # Completing SOS if needed (sections without pole or zero) if npc + ceil(npr / 2) > nzc + ceil(nzr / 2) for count = nzc + ceil (nzr / 2) + 1 : npc + ceil (npr / 2) % sections without zero SOS(count, 1:3) = [0, 0, 1]; endfor else for count = npc + ceil(npr / 2) + 1:nzc + ceil (nzr / 2) % sections without pole SOS(count, 4:6) = [0, 0, 1]; endfor endif endif if ~exist ('SOS') SOS=[0, 0, 1, 0, 0, 1]; # leading zeros will be removed endif # Removing leading zeros if present in numerator and denominator for count = 1:size(SOS,1) B=SOS(count, 1:3); A=SOS(count, 4:6); while B(1)==0 && A(1)==0 A(1) = []; # faster than A = circshift(A,-1); A(3) = 0; # " B(1) = []; B(3) = 0; endwhile SOS(count,:) = [B,A]; endfor ## If no output argument for the overall gain, combine it into the ## first section. if (nargout < 2) SOS(1,1:3) = k * SOS(1,1:3); else G = k; endif %!test %! B=[1 0 0 0 0 1]; A=[1 0 0 0 0 .9]; %! [z,p,k] = tf2zp(B,A); %! [sos,g] = zp2sos(z,p,k); %! [Bh,Ah] = sos2tf(sos,g); %! assert({Bh,Ah},{B,A},100*eps); %!test %! sos = zp2sos ([]); %! assert (sos, [1, 0, 0, 1, 0, 0], 100*eps); %!test %! sos = zp2sos ([], []); %! assert (sos, [1, 0, 0, 1, 0, 0], 100*eps); %!test %! sos = zp2sos ([], [], 2); %! assert (sos, [2, 0, 0, 1, 0, 0], 100*eps); %!test %! [sos, g] = zp2sos ([], [], 2); %! assert (sos, [1, 0, 0, 1, 0, 0], 100*eps); %! assert (g, 2, 100*eps); %!test %! sos = zp2sos([], [0], 1); %! assert (sos, [0, 1, 0, 1, 0, 0], 100*eps); %!test %! sos = zp2sos([0], [], 1); %! assert (sos, [1, 0, 0, 0, 1, 0], 100*eps); %!test %! sos = zp2sos([-1-j -1+j], [-1-2j -1+2j], 10); %! assert (sos, [10, 20, 20, 1, 2, 5], 100*eps);