home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / tzero < prev    next >
Encoding:
Text File  |  1995-11-15  |  4.1 KB  |  155 lines
  1. //---------------------------------------------------------------------------------
  2. //
  3. // tzero
  4. //
  5. // Syntax: A=tzero(a,b,c,d)
  6. //
  7. // This routine computes the transmission zeros of the state-space system
  8. // defined by (a,b,c,d). Calling the routine as Z=tzero(a,b,c,d) returns the
  9. // transmission zeros of state-space system (either continuous or discrete)
  10. // given by,
  11. //                       .                  .
  12. //                       x = Ax + Bu   or   x[n+1] = Ax[n] + Bu[n]
  13. //                       y = Cx + Du          y[n] = Cx[n] + Du[n]
  14. //
  15. // If the system is SISO, then the transfer funtion gains will also be
  16. // returned (as part of the return list).
  17. //
  18. // It extracts from the system matrix of a state-space system [ A B C D ] a
  19. // regular pencil [ Lambda*Bf - Af ] which has the NU Invariant Zeros of the
  20. // system as Generalized Eigenvalues.
  21. //  
  22. // REFERENCE: Ported to RLaB from the FORTRAN Code contained in:
  23. // Emami-Naeini, A., and Van Dooren, A., "Computation of Zeros of Linear
  24. // Multivaraible Systems," Automatica, Vol. 18, No. 4, April 1982, pp. 415-430.
  25. //
  26. // The transmission zero calculation can be tested by checking that the
  27. // matrix: [A B;C D]-lambda*[I 0; 0 0] is rank deficient, where lambda
  28. // is an element of Z.
  29. //
  30. // The results are returned in a list:
  31. //
  32. // A.z= z (transmission zeros)
  33. // A.gain = gain (transfer function gain if SISO)
  34. //
  35. // Copyright (C), by Jeffrey B. Layton, 1994
  36. // Version JBL 931205
  37. //---------------------------------------------------------------------------------
  38.  
  39. rfile isempty
  40. rfile abcdchk
  41. rfile tzreduce
  42. rfile housh
  43.  
  44. tzero = function(a,b,c,d)
  45. {
  46.    local(msg,estr,nn,nx,pp,mmtf,z,gain,bf,A,Zeps,mm,tf,mu,nu,Rank,numu,mnu,...
  47.          af,nu1,i0,i1,i,dummy,s,zero,cols)
  48.  
  49. // Check if (a,b,c,d) is correct
  50.    msg=abcdchk(a,b,c,d);
  51.    if (msg != "") {
  52.        estr="TZERO: "+msg;
  53.        error(estr);
  54.    }
  55.  
  56. // Special Case of an empty b and c matrices
  57.    if ( (isempty(b)) && (isempty(c))) {
  58.        z=[];
  59.        gain
  60.        return << z=z; gain=gain >>
  61.    }
  62.  
  63.    Zeps=2*epsilon()*norm(a,"fro");
  64.  
  65.    nn=a.nr;
  66.    nx=a.nc;
  67.    pp=c.nr;
  68.    mm=b.nc;
  69.    if ( (pp*mm) == 1) {
  70.        tf=1;
  71.    else
  72.        tf=0;
  73.    }
  74.  
  75. //* Construct the Compound Matrix [ B A ] of Dimension (N+P)x(M+N) 
  76. //*                               [ D C ]
  77.  
  78.    bf=[b,a;d,c];
  79.  
  80. //* Reduce this system to one with the same invariant zeros and with
  81. //* D full rank MU (The Normal Rank of the original system).
  82. //*
  83.  
  84.    A=tzreduce(bf,mm,nn,pp,Zeps,pp,0);
  85.    bf=A.abcd;
  86.    mu=A.mu;
  87.    nu=A.nu;
  88.    Rank=mu;
  89.  
  90.    if (nu == 0) {
  91.        z=[];
  92.    else
  93. //* Pretranspose the system 
  94.        numu=nu+mu;
  95.        mnu=mm+nu;
  96.        af=zeros(mnu,numu);
  97.        af[mnu:1:-1;numu:-1:1]=bf[1:numu;1:mnu]';
  98.  
  99.        if (mu != mm) {
  100.          pp=mm;
  101.          nn=nu;
  102.          mm=mu;
  103.  
  104. //* Reduce the system to one with the same invariant zeros and with
  105. //* D square invertible.
  106.  
  107.          A=tzreduce(af,mm,nn,pp,Zeps,pp-mm,mm);
  108.          af=A.abcd;
  109.          mu=A.mu;
  110.          nu=A.nu;
  111.          mnu=mm+nu;
  112.        }
  113.  
  114.        if (nu == 0) {
  115.            z=[];
  116.        else
  117. //* Perform a unitary transformation on the columns of [ sI-A B ] in
  118. //*                       [ sBf-Af X ]                 [   -C D ]
  119. //* order to reduce it to [   0    Y ] with Y and Bf square invertible.
  120.  
  121.            bf[1:nu;1:mnu]=[zeros(nu,mm),eye(nu,nu)];
  122.            if (Rank!= 0) {
  123.                nu1=nu+1;
  124.                i1=nu+mu;
  125.                i0 = mm;
  126.                for (i in 1:mm) {
  127.                     i0=i0-1;
  128.                     cols=i0+[1:nu1];
  129.                     dummy=af[i1;cols];
  130.                     A=housh(dummy,nu1,Zeps);
  131.                     dummy=A.u;
  132.                     s=A.s;
  133.                     zero=A.zero;
  134.                     af[1:i1;cols]=af[1:i1;cols]*(eye(nu1,nu1)-s*dummy*dummy');
  135.                     bf[1:nu;cols]=bf[1:nu;cols]*(eye(nu1,nu1)-s*dummy*dummy');
  136.                     i1=i1-1;
  137.                }
  138.            }
  139.  
  140. //* Solve Generalized zeros of sBF - AF
  141.        z=eig(af[1:nu;1:nu]/bf[1:nu;1:nu]).val;
  142.        }
  143.    }
  144.  
  145.    if (tf) {
  146.        if (nu == nx) {
  147.            gain=bf[nu+1;1];
  148.        else
  149.            gain=bf[nu+1;1]*prod(diag(bf[nu+2:nx+1;nu+2:nx+1]));
  150.        }
  151.    }
  152.  
  153.    return << z=z; gain=gain >>
  154. };
  155.