% pfe.m
% Test Matlab script to illustrate converting a direct-form filter to 
% a bank of parallel (complex) one-pole filters, and then to a bank
% of real second-order sections (biquads).
%
% 11/21/00
% J.O. Smith
% for Music 320 lab presentation on IIR filters

order = 5; % Experiment with a variety of orders

dofigs=1; % set to 0 to suppress figures (useful over a DSL connection)
dopause=1; % set to 1 to pause after each display, else 0
figstep=1-dopause; % set to 1 to keep all figures lying around

B = 1; % Numerator polynomial
poleradius = 0.9;
% Denominator polynomial (IIR comb filter):
A = [1,zeros(1,order-1),poleradius^(1/order)];
format short;
A
cfig = 1;
if dofigs
  figure(cfig); cfig=cfig+figstep;
  myfreqz(B,A); % Frequency response
  subplot(2,1,1);
  title('Frequency response of direct-form filter');
  if dopause, disp('*** PAUSING *** (press RETURN to continue)'); pause; end


  figure(cfig); cfig=cfig+figstep;
  subplot(1,1,1);
  zplane(0,roots(A)); % Plot poles (no zeros)
  title('Pole-Zero plot of test filter');
  if dopause, disp('*** PAUSING *** (press RETURN to continue)'); pause; end
end

disp('\n=== Doing PARTIAL FRACTION EXPANSION ===');

[residues,poles,firpart] = residuez(B,A); % Partial Fraction Expansion
disp('residues:'); residues
disp('poles:'); poles
disp('FIR remainder:'); firpart

N=100;  % samples to display over time
delta = [1,zeros(1,N-1)]; % Impulse signal
h1 = filter(B,A,delta); % Impulse response of direct form

hn = zeros(order,N);
for n=1:order
  Bn = residues(n); % n'th residue
  An = [1 -poles(n)]; % complex one-pole denominator polynomial
  hn(n,:) = filter(Bn,An,delta); % Impulse response of complex one-pole

  if dofigs
    % Display impulse response of this complex 1-pole section
    figure(cfig); cfig=cfig+figstep;
    subplot(2,1,1);
    plot(real(hn(n,:)),':o'); grid;
    xlabel('Time (samples)');
    ylabel('Real part');
    title(sprintf('Impulse response of complex one-pole section %d',n));
    subplot(2,1,2);
    plot(imag(hn(n,:)),':o'); grid;
    xlabel('Time (samples)');
    ylabel('Imag part');
    if dopause
      disp('*** PAUSING *** (press RETURN to continue)'); 
      pause; 
    end

    % Now display frequency response
    figure(cfig); cfig=cfig+figstep;
    subplot(1,1,1);
    % Take a look - must see whole circle
    myfreqz(Bn,An,512,'whole');
    subplot(2,1,1); title(sprintf(...
      'Frequency response of complex one-pole section %d',n));
    if dopause
      disp('*** PAUSING *** (press RETURN to continue)'); 
      pause; 
    end
  end
end

h2 = sum(hn,1); % sum of complex-one-pole impulse responses
disp(sprintf('Imag part of PFE sum is %0.5f%% of total\n',...
    100*norm(imag(h2))/norm(h2)));
h2 = real(h2);

if dofigs
  figure(cfig); cfig=cfig+figstep;

  subplot(2,1,1);
  stem(h1); grid;
  ylabel('Amplitude');
  title('Impulse response of direct form filter');
 
  subplot(2,1,2);
  stem(h2); grid;
  xlabel('Time (samples)');
  ylabel('Amplitude');
  title('Impulse response of parallel one-pole filter bank');
  if dopause, disp('*** PAUSING *** (press RETURN to continue)'); pause; end
end
disp(sprintf(...
    'Direct-form minus PFE impulse-response error is %0.5f%%\n',...
    100*norm(h1-h2)/norm(h1)));

disp('=== Combining COMPLEX ONE-POLE RESONATORS into REAL BIQUAD SECTIONS ===');

% If the original filter is real, then the one-pole sections will
% occur in complex-conjugate pairs which can be combined into real
% second-order sections.

% Convert now to a real, parallel, filter-bank consisting of either
% first-order or second-order filter sections in the following format:
%
% realbiquads(1).B == [b11 b12 b13]
% realbiquads(1).A == [1 a11 a12]
% realbiquads(1).order == 1 or 2
%
% realbiquads(2).B == [b12 b22 b23]
% realbiquads(2).A == [1 a21 a22]
% realbiquads(2).order == 1 or 2
%
% ...
% realbiquads(nsecs).order == 1 or 2.
%
% (Note that "structs" were introduced in Matlab 5.)

nsecs=0;
r2 = residues;
p2 = poles;
while r2
  cr = r2(1); % current residue
  cp = p2(1); % current pole
  if (isreal(cp)) % real one-pole section
    nsecs = nsecs + 1;
    realbiquads(nsecs).order = 1;
    realbiquads(nsecs).B = [cr];
    realbiquads(nsecs).A = [1, -cp];
    r2 = r2(2:end); % delete processed residue
    p2 = p2(2:end); % delete processed pole
  else % complex two-pole section
    nsecs = nsecs + 1;
    realbiquads(nsecs).order = 2;
    realbiquads(nsecs).B = [2*real(cr), -2*real(cr*conj(cp))];
    realbiquads(nsecs).A = [1, -2*real(cp), abs(cp)^2];
    r2 = r2(2:end); % delete processed residue
    p2 = p2(2:end); % delete processed pole
    crci = find(abs(r2-conj(cr))<1E-8); % Find complex-conjugate partner
    if (~crci), error('pfe: could not find complex conjugate partner'); end
    r2 = [r2(1:crci-1),r2(crci+1:end)]; % delete processed residues
    p2 = [p2(1:crci-1),p2(crci+1:end)]; % delete processed poles
  end
end

realbiquads
if dopause, disp('*** PAUSING *** (press RETURN to continue)'); pause; end

for i=1:nsecs
  disp(sprintf('Section %d',i));
  realbiquads(i)
end
if dopause, disp('*** PAUSING *** (press RETURN to continue)'); pause; end

hi = zeros(nsecs,N);
for i=1:nsecs
  Bi = realbiquads(i).B;
  Ai = realbiquads(i).A;
  hi(i,:) = filter(Bi,Ai,delta); % Impulse response of i'th section
  if dofigs
    figure(cfig); cfig=cfig+figstep;
    subplot(1,1,1);
    plot(hi(i,:),':o'); grid;
    xlabel('Time (samples)');
    ylabel('Amplitude');
    title(sprintf('Impulse response of real biquad section %d',i));
    if dopause, disp('*** PAUSING *** (press RETURN to continue)'); pause; end
  end
end
h3 = sum(hi,1); % sum of biquad impulse responses

if dofigs
  figure(cfig); cfig=cfig+figstep;
  subplot(2,1,1);
  stem(h1); grid;
  ylabel('Amplitude');
  title('Impulse response of direct form filter');

  subplot(2,1,2);
  stem(h3); grid;
  xlabel('Time (samples)');
  ylabel('Amplitude');
  title('Impulse response of parallel biquad filter bank');
  if dopause, disp('*** PAUSING *** (press RETURN to continue)'); pause; end
end
disp(sprintf(...
    'Direct-form minus BiquadBank impulse-response error is %0.5f%%',...
    100*norm(h1-h3)/norm(h1)));