home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / aremsf < prev    next >
Encoding:
Text File  |  1995-11-15  |  2.9 KB  |  120 lines
  1. //--------------------------------------------------------------------------------
  2. //
  3. // aremsf
  4. //
  5. // Syntax: D=aremsf(A,B,C,eps,itermax)
  6. //
  7. // This routine solves the Algebraic Riccati Equation using the matrix
  8. // Sign function.
  9. //
  10. //   A'P + PA - PBP + C = 0
  11. //
  12. // The input eps is the tolerance for the iteration procedure and the
  13. // variable itermax is the maximum number of iterations.
  14. //
  15. // The routine returns the steady-state continuous ARE solution.
  16. //
  17. //  Ref.: (1) Chapter 5 of Junkins & Kim, Dyn. & Ctrl. of Structures.
  18. //        (2) Bierman, G.J.,
  19. //            "Computational Aspects of the Matrix Sign Function to the ARE"
  20. //            Proc. 23rd CDC, 1984, pp.514-519.
  21. //
  22. // Copyright (C), by Jeffrey B. Layton, 1994
  23. // Version JBL 940918
  24. //--------------------------------------------------------------------------------
  25.  
  26. aremsf = function(A,B,C,Reps,iterm)
  27. {
  28.    local(itermax,eps,n,i,msg,estr,Jhat,Hhat,Hhatnew,SignH,W,...
  29.          W11,W12,P)
  30.  
  31. // Count number of input arguments
  32.    nargs=0;
  33.    if (exist(A)) {nargs=nargs+1;}
  34.    if (exist(B)) {nargs=nargs+1;}
  35.    if (exist(C)) {nargs=nargs+1;}
  36.    if (exist(Reps)) {nargs=nargs+1;}
  37.    if (exist(iterm)) {nargs=nargs+1;}
  38.  
  39. // Error Traps
  40. // ===========
  41.  
  42. // Check if A and C are consistent.
  43.    if ( A.nr != C.nr || A.nc != C.nc ) {
  44.        error("AREMSF: A and C must be consistent");
  45.    }
  46.    
  47.    if ( A.nr != A.nc || B.nc != A.nr ) {
  48.        error("AREMSF: A and B must be consistent");
  49.    }
  50.    
  51. // Check if A and B are empty
  52.    if ( (!length(A)) || (!length(B)) ) {
  53.        error("AREMSF: A and B matrices cannot be empty.");
  54.    }
  55.  
  56. // A has to be square
  57.    if (A.nr != A.nc) {
  58.        error("AREMSF: A has to be square.");
  59.    }
  60.  
  61. // See if A and B are compatible.
  62.    msg="";
  63.    msg=abcdchk(A,B);
  64.    if (msg != "") {
  65.        estr="AREMSF: "+msg;
  66.        error(estr);
  67.    }
  68.  
  69. // Check if C is positive semi-definite and symmetric
  70.    if (!issymm(C)) {
  71.        printf("%s","aremsf: Warning: C is not symmetric.\n");
  72.    else
  73.        if (any(eig(C).val < -epsilon()*norm(C,"1")) ) { 
  74.            printf("%s","aremsf: Warning: C is not positive semi-definite.\n");
  75.        }
  76.    }
  77.  
  78. // Set defaults if variables are not input.
  79. // ========================================
  80.  
  81.    if (nargs < 5) {
  82.        itermax=1000;
  83.    else
  84.        itermax=iterm;
  85.    }
  86.  
  87.    if (nargs < 4) {
  88.        eps=1.0e-10;
  89.    else
  90.        eps=Reps
  91.    }
  92.  
  93.    n=A.nr;
  94.  
  95. //  Iteration begins for computing matrix sign function of Hamiltonian
  96. //  (Use transformed Hamiltonian to get accurate inverse of Hamiltonian)
  97.  
  98.    Jhat=[zeros(n,n),-eye(n,n);eye(n,n),zeros(n,n)];
  99.    Hhat=[C, A';A, -B];
  100.  
  101.    for (i in 1:itermax) {
  102.         Hhatnew=0.5*(Hhat+Jhat*inv(Hhat)*Jhat);
  103.         if (norm(Hhatnew-Hhat,"f") < eps) {
  104.             break;
  105.         }
  106.         Hhat=Hhatnew;
  107.    }
  108.  
  109.    SignH=-Jhat*Hhatnew;
  110.  
  111. //  Compute the Riccati Solution
  112.  
  113.    W=0.5*(SignH+eye(2*n,2*n));
  114.    W12=W[1:n;n+1:2*n];
  115.    W11=W[1:n;1:n];
  116.    P=-inv(W12)*W11;
  117.  
  118.    return P
  119. };
  120.