octave_packages/signal-1.1.3/zp2sos.m

   1 %% Copyright (C) 2005 Julius O. Smith III <jos@ccrma.stanford.edu>
   2 %%
   3 %% This program is free software; you can redistribute it and/or modify it under
   4 %% the terms of the GNU General Public License as published by the Free Software
   5 %% Foundation; either version 3 of the License, or (at your option) any later
   6 %% version.
   7 %%
   8 %% This program is distributed in the hope that it will be useful, but WITHOUT
   9 %% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  10 %% FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
  11 %% details.
  12 %%
  13 %% You should have received a copy of the GNU General Public License along with
  14 %% this program; if not, see <http://www.gnu.org/licenses/>.
  15
  16 %% -*- texinfo -*-
  17 %% @deftypefn {Function File} {[@var{sos}, @var{g}] =} zp2sos (@var{z}, @var{p})
  18 %% Convert filter poles and zeros to second-order sections.
  19 %%
  20 %% INPUTS:@*
  21 %% @itemize
  22 %% @item
  23 %%   @var{z} = column-vector containing the filter zeros@*
  24 %% @item
  25 %%   @var{p} = column-vector containing the filter poles@*
  26 %% @item
  27 %%   @var{g} = overall filter gain factor
  28 %% @end itemize
  29 %%
  30 %% RETURNED:
  31 %% @itemize
  32 %% @item
  33 %% @var{sos} = matrix of series second-order sections, one per row:@*
  34 %% @var{sos} = [@var{B1}.' @var{A1}.'; ...; @var{BN}.' @var{AN}.'], where@*
  35 %% @code{@var{B1}.'==[b0 b1 b2] and @var{A1}.'==[1 a1 a2]} for
  36 %% section 1, etc.@*
  37 %% b0 must be nonzero for each section.@*
  38 %% See @code{filter()} for documentation of the
  39 %% second-order direct-form filter coefficients @var{B}i and
  40 %% %@var{A}i, i=1:N.
  41 %%
  42 %% @item
  43 %% @var{Bscale} is an overall gain factor that effectively scales
  44 %% any one of the @var{B}i vectors.
  45 %% @end itemize
  46 %%
  47 %% EXAMPLE:
  48 %% @example
  49 %%   [z,p,g] = tf2zp([1 0 0 0 0 1],[1 0 0 0 0 .9]);
  50 %%   [sos,g] = zp2sos(z,p,g)
  51 %%
  52 %% sos =
  53 %%    1.0000    0.6180    1.0000    1.0000    0.6051    0.9587
  54 %%    1.0000   -1.6180    1.0000    1.0000   -1.5843    0.9587
  55 %%    1.0000    1.0000         0    1.0000    0.9791         0
  56 %%
  57 %% g =
  58 %%     1
  59 %% @end example
  60 %%
  61 %% @seealso{sos2pz sos2tf tf2sos zp2tf tf2zp}
  62 %% @end deftypefn
  63
  64 function [sos,g] = zp2sos(z,p,g)
  65
  66   if nargin<3, g=1; end
  67   if nargin<2, p=[]; end
  68
  69   [zc,zr] = cplxreal(z);
  70   [pc,pr] = cplxreal(p);
  71
  72   % zc,zr,pc,pr
  73
  74   nzc=length(zc);
  75   npc=length(pc);
  76
  77   nzr=length(zr);
  78   npr=length(pr);
  79
  80   % Pair up real zeros:
  81   if nzr
  82     if mod(nzr,2)==1, zr=[zr;0]; nzr=nzr+1; end
  83     nzrsec = nzr/2;
  84     zrms = -zr(1:2:nzr-1)-zr(2:2:nzr);
  85     zrp = zr(1:2:nzr-1).*zr(2:2:nzr);
  86   else
  87     nzrsec = 0;
  88   end
  89
  90   % Pair up real poles:
  91   if npr
  92     if mod(npr,2)==1, pr=[pr;0]; npr=npr+1; end
  93     nprsec = npr/2;
  94     prms = -pr(1:2:npr-1)-pr(2:2:npr);
  95     prp = pr(1:2:npr-1).*pr(2:2:npr);
  96   else
  97     nprsec = 0;
  98   end
  99
 100   nsecs = max(nzc+nzrsec,npc+nprsec);
 101
 102   % Convert complex zeros and poles to real 2nd-order section form:
 103   zcm2r = -2*real(zc);
 104   zca2 = abs(zc).^2;
 105   pcm2r = -2*real(pc);
 106   pca2 = abs(pc).^2;
 107
 108   sos = zeros(nsecs,6);
 109   sos(:,1) = ones(nsecs,1); % all 2nd-order polynomials are monic
 110   sos(:,4) = ones(nsecs,1);
 111
 112   nzrl=nzc+nzrsec; % index of last real zero section
 113   nprl=npc+nprsec; % index of last real pole section
 114
 115   for i=1:nsecs
 116
 117     if i<=nzc % lay down a complex zero pair:
 118       sos(i,2:3) = [zcm2r(i) zca2(i)];
 119     elseif i<=nzrl % lay down a pair of real zeros:
 120       sos(i,2:3) = [zrms(i-nzc) zrp(i-nzc)];
 121     end
 122
 123     if i<=npc % lay down a complex pole pair:
 124       sos(i,5:6) = [pcm2r(i) pca2(i)];
 125     elseif i<=nprl % lay down a pair of real poles:
 126       sos(i,5:6) = [prms(i-npc) prp(i-npc)];
 127     end
 128   end
 129 end
 130
 131 %!test
 132 %! B=[1 0 0 0 0 1]; A=[1 0 0 0 0 .9];
 133 %! [z,p,g] = tf2zp(B,A);
 134 %! [sos,g] = zp2sos(z,p,g);
 135 %! [Bh,Ah] = sos2tf(sos,g);
 136 %! assert({Bh,Ah},{B,A},100*eps);