home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / PLACE.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  4.0 KB  |  146 lines
  1. function K = place(A,B,P)
  2. %PLACE    K = place(A,B,P)  computes the state feedback matrix K such that
  3. %    the eigenvalues of  A-B*K  are those specified in vector P.
  4. %    The complex eigenvalues in the vector P must appear in consecu-
  5. %    tive complex conjugate pairs. No eigenvalue may be placed with
  6. %    multiplicity greater than the number of inputs.  
  7. %
  8. %    The  displayed "ndigits" is an  estimate of how well the
  9. %    eigenvalues were placed.   The value seems to give an estimate
  10. %    of how many decimal digits in the eigenvalues of A-B*K match
  11. %    the specified numbers given in the array P.
  12. %
  13. %    A warning message is printed if the nonzero closed loop poles
  14. %    are greater than 10% from the desired locations specified in P.
  15. %
  16. %    See also: LQR and RLOCUS.
  17.  
  18. %    M. Wette 10-1-86
  19. %    UCSB ECE, Santa Barbara, CA 93106, (805) 961-4691
  20. %          E-mail: riccati@hub.ucsb.edu
  21. %    Revised 9-25-87 JNL
  22. %       Revised 8-4-92 Wes Wang
  23. %
  24. %  Ref::    Kautsky, Nichols, Van Dooren, "Robust Pole Assignment in Linear 
  25. %           State Feedback," Intl. J. Control, 41(1985)5, pp 1129-1155
  26. %
  27.  
  28. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  29.  
  30. NTRY=5;         % - number of iterations for "optimization" -
  31. %
  32. [nx,na] = size(A);
  33.  
  34. P = P(:);
  35. if length(P)~=nx, error('P must have the same number of states as A.'); end
  36.  
  37. [n,m] = size(B);
  38. if (nx == 0 | n == 0), 
  39.     error('A and B matrices cannot be empty.')
  40. end
  41. nx=0; i=1; while (i<=n),
  42.     if imag(P(i))~=0.0,
  43.         pr = [pr real(P(i))]; pi = [pi imag(P(i))];
  44.         cmplx = [cmplx 1]; i = i+2;
  45.     else,
  46.         pr = [pr real(P(i))]; pi = [pi 0.0];
  47.         cmplx = [cmplx 0]; i = i+1;
  48.     end;
  49.     nx = nx+1;
  50. end;
  51. %
  52. m = rank(B);
  53. % Make sure there are more inputs than repeated poles:
  54. ps = sort(P);
  55. for i=1:n-m
  56.     imax = min(n,i+m);
  57.     if all(ps(i:imax) == ps(i))
  58. error('Can''t place poles with multiplicity greater than the number of inputs.');
  59.     end
  60. end
  61. nmmp1 = n-m+1; mp1 = m+1; jj = sqrt(-1);
  62. [Qb,Rb] = qr(B); q0 = Qb(:,1:m); q1 = Qb(:,mp1:n); Rb = Rb(1:m,:);
  63. %
  64. % - special case: (#inputs)==(#states) - efficient, but not clean
  65. if (m==n),
  66.     A = A - diag(real(P));
  67.     i=0; for j=1:nx,
  68.     i = i+1;
  69.     if cmplx(j),
  70.         A(i,i+1) = A(i,i+1) + pi(j);
  71.         A(i+1,i) = A(i+1,i) - pi(j);
  72.         i = i+1;
  73.     end;
  74.     end;
  75.     disp(sprintf('place: ndigits= %g', fix(log10(1.0/eps))))
  76.     K = Rb\q0'*A;
  77.     return;        % escape here!
  78. end
  79. %
  80. % - compute bases for eigenvectors -
  81. I = eye(n);
  82. for i=1:nx,
  83.     [Q,R] = qr(((pr(i)+jj*pi(i))*I-A)'*q1);
  84.     Bx = [ Bx Q(:,nmmp1:n) ];
  85. end;
  86. %
  87. % - choose basis set -
  88. % at each iteration of i pick the eigenvector Xj, j~=i, 
  89. % which is "most orthogonal" to the current eigenvector Xi
  90. %Wes changed the following
  91. nn=1; 
  92. for i=1:nx, 
  93.   X(:,i) = Bx(:,(i-1)*m+1); 
  94.   if m>1 %check if X is a full rank matrix. If it is not, make it up
  95.     for ii = 2:m
  96.       nnx = nn + cmplx(i);
  97.       Y(:,nnx) = imag(X(:,i)); %if cmplx(i)==1 take imag part, else empty action
  98.       Y(:,nn) = real(X(:,i));
  99.       if rank(Y) < nnx, 
  100.         X(:,i) = Bx(:,(i-1)*m+ii); 
  101.       else
  102.         ii = m;
  103.       end; % if rank(Y) < nnx, 
  104.     end; %for ii = 2:m
  105.     nn = nn + 1 + cmplx(i); 
  106.   end; %if m>1
  107. end; %for i=1:nx, 
  108. % Wes changed the above
  109. if (m>1),
  110.     for k = 1:NTRY,
  111.     for i = 1:nx,
  112.         S = [ X(:,1:i-1) X(:,i+1:nx) ]; S = [ S conj(S) ];
  113.         [Us,Ss,Vs] = svd(S);
  114.         Pr = Bx(:,(i-1)*m+1:i*m); Pr = Pr*Pr';
  115.         X(:,i) = Pr*Us(:,n); X(:,i) = X(:,i)/norm(X(:,i));
  116.     end
  117.     end
  118. end
  119. for i = 1:nx,
  120.     if cmplx(i),
  121.         Xf = [ Xf X(:,i) conj(X(:,i)) ];
  122.     else,
  123.         Xf = [ Xf X(:,i) ];
  124.     end;
  125. end;
  126. cnd = cond(Xf);
  127. if (cnd*eps >= 1.0),
  128.     disp('place: can''t place eigenvalues there');
  129.     return;
  130. end
  131. disp(sprintf('place: ndigits= %g', fix(log10(cnd/eps))))
  132. %
  133. % - compute feedback -
  134. K = Rb\q0'*(A-real(Xf*diag(P,0)/Xf));
  135.  
  136. % Check results. Start by removing 0.0 pole locations
  137. P = sort(P);
  138. i = find(P ~= 0);
  139. P = P(i);
  140. Pc = sort(eig(A-B*K));
  141. Pc = Pc(i);
  142. if max(abs(P-Pc)./abs(P)) > .1
  143.     disp('Warning: Pole locations are more than 10% in error.')
  144. end
  145. % --- last line of place ---
  146.