dlqr.m

数字通信第四版原书的例程

function [k,s,e] = dlqr(a,b,q,r,nn)
%DLQR	Linear quadratic regulator design for discrete-time systems.
%	[K,S,E] = DLQR(A,B,Q,R)  calculates the optimal feedback gain 
%	matrix K such that the feedback law  u[n] = -Kx[n]  minimizes the
%	cost function
%		J = Sum {x'Qx + u'Ru}
%	subject to the constraint equation:   
%		x[n+1] = Ax[n] + Bu[n] 
%                
%	Also returned is S, the steady-state solution to the associated 
%	discrete matrix Riccati equation and the closed loop eigenvalues
%	E,                            -1
%           0 = S - A'SA + A'SB(R+B'SB) BS'A - Q         E = EIG(A-B*K)
%
%	[K.S,E] = DLQR(A,B,Q,R,N) includes the cross-term N that relates 
%	u to x in the cost function:
%
%		J = Sum {x'Qx + u'Ru + 2*x'Nu}
%
%	The controller can be formed with DREG.
%
%	See also: DLQRY, LQRD, and DREG.

%	J.N. Little 4-21-85
%	Revised 6-23-86 JNL
%	Revised 7-16-90 Clay M. Thompson 
%	Copyright (c) 1986-93 by the MathWorks, Inc.

error(nargchk(4,5,nargin));
error(abcdchk(a,b));
if ~length(a) | ~length(b)
	error('A and B matrices cannot be empty.')
end

[m,n] = size(a);
[mb,nb] = size(b);
[mq,nq] = size(q);
if (m ~= mq) | (n ~= nq) 
	error('A and Q must be the same size');
end
[mr,nr] = size(r);
if (mr ~= nr) | (nb ~= mr)
	error('B and R must be consistent');
end

if nargin==5
  [mn,nnn] = size(nn);
  if (mn ~= m) | (nnn ~= nr), error('N must be consistent with Q and R'); end
  % Add cross term
  q = q - nn/r*nn';
  a = a - b/r*nn';
else
  nn = zeros(m,nb);
end

% Check if q is positive semi-definite and symmetric
nq = norm(q,1);
if any(eig(q) < -eps*nq) | (norm(q'-q,1)/nq > eps)
	disp('Warning: Q is not symmetric and positive semi-definite');
end
% Check if r is positive definite and symmetric
nr = norm(r,1);
if any(eig(r) <= -eps*nr) | (norm(r'-r,1)/nr > eps)
	disp('Warning: R is not symmetric and positive definite');
end

% eigenvectors of Hamiltonian
[v,d] = eig([a+b/r*b'/a'*q  -b/r*b'/a'; -a'\q  inv(a)']);

d = diag(d);
[e,index] = sort(abs(d));	 % sort on magnitude of eigenvalues
if (~((e(n) < 1) & (e(n+1)>1)))
	error('Can''t order eigenvalues, (A,B) may be uncontrollable.');
else
  e = d(index(1:n));
end
% select vectors with eigenvalues inside unit circle
chi = v(1:n,index(1:n));
lambda = v((n+1):(2*n),index(1:n));
s = real(lambda/chi);
k = (r+b'*s*b)\b'*s*a + r\nn';