home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / ZP2SS.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  4.0 KB  |  150 lines
  1. function [a,b,c,d] = zp2ss(z,p,k)
  2. %ZP2SS    Zero-pole to state-space conversion.
  3. %    [A,B,C,D] = ZP2SS(Z,P,K)  calculates a state-space representation:
  4. %        .
  5. %        x = Ax + Bu
  6. %        y = Cx + Du
  7. %
  8. %    for a system given a set of pole locations in column vector P,
  9. %    a matrix Z with the zero locations in as many columns as there are
  10. %    outputs, and the gains for each numerator transfer function in
  11. %    vector K.  The A,B,C,D matrices are returned in block diagonal
  12. %    form.
  13. %
  14. %       This function handles SIMO systems if the Control Toolbox is
  15. %       present and SISO systems if only the Signal Processing Toolbox
  16. %       is installed.
  17. %       
  18. %    See also SS2ZP.
  19.  
  20. %    J.N. Little & G.F. Franklin  8-4-87
  21. %    Revised 12-27-88 JNL, 12-8-89, 11-12-90, 3-22-91, A.Grace
  22. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  23.  
  24. [mn,nn] = size(z); [md,nd]=size(p);
  25. % Put it in column format if its SISO and in row format.
  26. if (length(k)==1 & md < 2 & mn < 2) & (nn > 1 | nd > 1) 
  27.     z = z'; p = p'; 
  28. end
  29. [m,n] = size(z);
  30. if n==0, n=length(k); end  % Fix to handle multi-output when z is empty
  31. if length(k) ~= n & (~isempty(z))
  32.     error('Z should be a column vector or K should be SIMO.');
  33. end
  34. if n > 1
  35.     % If it's multi-output, we can't use the nice algorithm
  36.     % that follows, so use the numerically unreliable method
  37.     % of going through polynomial form, and then return.
  38.     eval('[num,den] = zp2tf(z,p,k);') % Suppress compile-time diagnostics
  39.     [a,b,c,d] = tf2ss(num,den);
  40.     return
  41. end
  42.  
  43. % Strip infinities and throw away.
  44. p = p(finite(p));
  45. z = z(finite(z));
  46.  
  47. % Group into complex pairs
  48. np = length(p);
  49. nz = length(z);
  50. z = cplxpair(z,1e6*nz*norm(z)*eps + eps);
  51. p = cplxpair(p,1e6*np*norm(p)*eps + eps);
  52.  
  53. % Initialize state-space matrices for running series
  54. a=[]; b=[]; c=[]; d=1;
  55.  
  56. % If odd number of poles AND zeros, convert the pole and zero
  57. % at the end into state-space.
  58. %    H(s) = (s-z1)/(s-p1) = (s + num(2)) / (s + den(2))
  59. if rem(np,2) & rem(nz,2)
  60.     a = p(np);
  61.     b = 1;
  62.     c = p(np) - z(nz);
  63.     d = 1;
  64.     np = np - 1;
  65.     nz = nz - 1;
  66. end
  67.  
  68. % If odd number of poles only, convert the pole at the
  69. % end into state-space.
  70. %  H(s) = 1/(s-p1) = 1/(s + den(2)) 
  71. if rem(np,2)
  72.     a = p(np);
  73.     b = 1;
  74.     c = 1;
  75.     d = 0;
  76.     np = np - 1;
  77. end    
  78.  
  79. % If odd number of zeros only, convert the zero at the
  80. % end, along with a pole-pair into state-space.
  81. %   H(s) = (s+num(2))/(s^2+den(2)s+den(3)) 
  82. if rem(nz,2)
  83.     num = real(poly(z(nz)));
  84.     den = real(poly(p(np-1:np)));
  85.     wn = sqrt(prod(abs(p(np-1:np))));
  86.     if wn == 0, wn = 1; end
  87.     t = diag([1 1/wn]);    % Balancing transformation
  88.     a = t\[-den(2) -den(3); 1 0]*t;
  89.     b = t\[1; 0];
  90.     c = [1 num(2)]*t;
  91.     d = 0;
  92.     nz = nz - 1;
  93.     np = np - 2;
  94. end
  95.  
  96. % Now we have an even number of poles and zeros, although not 
  97. % necessarily the same number - there may be more poles.
  98. %   H(s) = (s^2+num(2)s+num(3))/(s^2+den(2)s+den(3))
  99. % Loop thru rest of pairs, connecting in series to build the model.
  100. i = 1;
  101. while i < nz
  102.     index = i:i+1;
  103.     num = real(poly(z(index)));
  104.     den = real(poly(p(index)));
  105.     wn = sqrt(prod(abs(p(index))));
  106.     if wn == 0, wn = 1; end
  107.     t = diag([1 1/wn]);    % Balancing transformation
  108.     a1 = t\[-den(2) -den(3); 1 0]*t;
  109.     b1 = t\[1; 0];
  110.     c1 = [num(2)-den(2) num(3)-den(3)]*t;
  111.     d1 = 1;
  112. %    [a,b,c,d] = series(a,b,c,d,a1,b1,c1,d1); 
  113. % Next lines perform series connection 
  114.     [ma1,na1] = size(a);
  115.     [ma2,na2] = size(a1);
  116.     a = [a zeros(ma1,na2); b1*c a1];
  117.     b = [b; b1*d];
  118.     c = [d1*c c1];
  119.     d = d1*d;
  120.  
  121.     i = i + 2;
  122. end
  123.  
  124. % Take care of any left over unmatched pole pairs.
  125. %   H(s) = 1/(s^2+den(2)s+den(3))
  126. while i < np
  127.     den = real(poly(p(i:i+1)));
  128.     wn = sqrt(prod(abs(p(i:i+1))));
  129.     if wn == 0, wn = 1; end
  130.     t = diag([1 1/wn]);    % Balancing transformation
  131.     a1 = t\[-den(2) -den(3); 1 0]*t;
  132.     b1 = t\[1; 0];
  133.     c1 = [0 1]*t;
  134.     d1 = 0;
  135. %    [a,b,c,d] = series(a,b,c,d,a1,b1,c1,d1);
  136. % Next lines perform series connection 
  137.     [ma1,na1] = size(a);
  138.     [ma2,na2] = size(a1);
  139.     a = [a zeros(ma1,na2); b1*c a1];
  140.     b = [b; b1*d];
  141.     c = [d1*c c1];
  142.     d = d1*d;
  143.  
  144.     i = i + 2;
  145. end
  146.  
  147. % Apply gain k:
  148. c = c*k;
  149. d = d*k;
  150.