1## Copyright (C) 2007 R.G.H. Eschauzier <reschauzier@yahoo.com>
2## Copyright (C) 2011 Carnë Draug <carandraug+dev@gmail.com>
3## Copyright (C) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch>
4##
5## This program is free software: you can redistribute it and/or modify
6## it under the terms of the GNU General Public License as published by
7## the Free Software Foundation, either version 3 of the License, or
8## (at your option) any later version.
9##
10## This program is distributed in the hope that it will be useful,
11## but WITHOUT ANY WARRANTY; without even the implied warranty of
12## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13## GNU General Public License for more details.
14##
15## You should have received a copy of the GNU General Public License
16## along with this program; see the file COPYING. If not, see
17## <https://www.gnu.org/licenses/>.
18
19## -*- texinfo -*-
20## @deftypefn  {Function File} {[@var{b_out}, @var{a_out}] =} impinvar (@var{b}, @var{a}, @var{fs}, @var{tol})
21## @deftypefnx {Function File} {[@var{b_out}, @var{a_out}] =} impinvar (@var{b}, @var{a}, @var{fs})
22## @deftypefnx {Function File} {[@var{b_out}, @var{a_out}] =} impinvar (@var{b}, @var{a})
23## Converts analog filter with coefficients @var{b} and @var{a} to digital,
24## conserving impulse response.
25##
26## If @var{fs} is not specified, or is an empty vector, it defaults to 1Hz.
27##
28## If @var{tol} is not specified, it defaults to 0.0001 (0.1%)
29## This function does the inverse of impinvar so that the following example should
30## restore the original values of @var{a} and @var{b}.
31##
32## @command{invimpinvar} implements the reverse of this function.
33## @example
34## [b, a] = impinvar (b, a);
35## [b, a] = invimpinvar (b, a);
36## @end example
37##
38## Reference: Thomas J. Cavicchi (1996) ``Impulse invariance and multiple-order
39## poles''. IEEE transactions on signal processing, Vol 44 (9): 2344--2347
40##
41## @seealso{bilinear, invimpinvar}
42## @end deftypefn
43
44function [b_out, a_out] = impinvar (b_in, a_in, fs = 1, tol = 0.0001)
45
46  if (nargin <2)
47    print_usage;
48  endif
49
50  ## to be compatible with the matlab implementation where an empty vector can
51  ## be used to get the default
52  if (isempty(fs))
53    ts = 1;
54  else
55    ts = 1/fs; # we should be using sampling frequencies to be compatible with Matlab
56  endif
57
58  [r_in, p_in, k_in] = residue(b_in, a_in); # partial fraction expansion
59
60  n = length(r_in); # Number of poles/residues
61
62  if (length(k_in)>0) # Greater than zero means we cannot do impulse invariance
63    error("Order numerator >= order denominator");
64  endif
65
66  r_out = zeros(1,n); # Residues of H(z)
67  p_out = zeros(1,n); # Poles of H(z)
68  k_out = 0;          # Constant term of H(z)
69
70  i=1;
71  while (i<=n)
72    m = 1;
73    first_pole = p_in(i); # Pole in the s-domain
74    while (i<n && abs(first_pole-p_in(i+1))<tol) # Multiple poles at p(i)
75      i++; # Next residue
76      m++; # Next multiplicity
77    endwhile
78    [r, p, k]        = z_res(r_in(i-m+1:i), first_pole, ts); # Find z-domain residues
79    k_out           += k;                                    # Add direct term to output
80    p_out(i-m+1:i)   = p;                                    # Copy z-domain pole(s) to output
81    r_out(i-m+1:i)   = r;                                    # Copy z-domain residue(s) to output
82
83    i++; # Next s-domain residue/pole
84  endwhile
85
86  [b_out, a_out] = inv_residue(r_out, p_out, k_out, tol);
87  a_out          = to_real(a_out); # Get rid of spurious imaginary part
88  b_out          = to_real(b_out);
89
90  ## Shift results right to account for calculating in z instead of z^-1
91  b_out(end)=[];
92
93endfunction
94
95## Convert residue vector for single and multiple poles in s-domain (located at sm) to
96## residue vector in z-domain. The variable k is the direct term of the result.
97
98function [r_out, p_out, k_out] = z_res (r_in, sm, ts)
99
100  p_out = exp(ts * sm); # z-domain pole
101  n     = length(r_in); # Multiplicity of the pole
102  r_out = zeros(1,n);   # Residue vector
103
104  ## First pole (no multiplicity)
105  k_out    = r_in(1) * ts;         # PFE of z/(z-p) = p/(z-p)+1; direct part
106  r_out(1) = r_in(1) * ts * p_out; # pole part of PFE
107
108  for i=(2:n) # Go through s-domain residues for multiple pole
109    r_out(1:i) += r_in(i) * polyrev(h1_z_deriv(i-1, p_out, ts)); # Add z-domain residues
110  endfor
111
112endfunction
113
114
115%!function err = stozerr(bs,as,fs)
116%!
117%!  # number of time steps
118%!  n=100;
119%!
120%!  # impulse invariant transform to z-domain
121%!  [bz az]=impinvar(bs,as,fs);
122%!
123%!  # create sys object of transfer function
124%!  s=tf(bs,as);
125%!
126%!  # calculate impulse response of continuous time system
127%!  # at discrete time intervals 1/fs
128%!  ys=impulse(s,(n-1)/fs,1/fs)';
129%!
130%!  # impulse response of discrete time system
131%!  yz=filter(bz,az,[1 zeros(1,n-1)]);
132%!
133%!  # find rms error
134%!  err=sqrt(sum((yz*fs.-ys).^2)/length(ys));
135%!  endfunction
136%!
137%!assert(stozerr([1],[1 1],100),0,0.0001);
138%!assert(stozerr([1],[1 2 1],100),0,0.0001);
139## FIXME: The following test needs a wider tolerance on some systems
140##        (arm64, i386, powerpc). Is this a problem in this function or
141##        in the control package that needs to be fixed?
142%!assert(stozerr([1 1],[1 2 1],100),0,0.0002);
143%!assert(stozerr([1],[1 3 3 1],100),0,0.0001);
144%!assert(stozerr([1 1],[1 3 3 1],100),0,0.0001);
145%!assert(stozerr([1 1 1],[1 3 3 1],100),0,0.0001);
146%!assert(stozerr([1],[1 0 1],100),0,0.0001);
147%!assert(stozerr([1 1],[1 0 1],100),0,0.0001);
148%!assert(stozerr([1],[1 0 2 0 1],100),0,0.0001);
149%!assert(stozerr([1 1],[1 0 2 0 1],100),0,0.0001);
150%!assert(stozerr([1 1 1],[1 0 2 0 1],100),0,0.0001);
151%!assert(stozerr([1 1 1 1],[1 0 2 0 1],100),0,0.0001);
152