function [k,s] = dlqr(a,b,q,r)
%DLQR	Linear quadratic regulator design for discrete-time systems.
%	[K,S] = DLQR(A,B,Q,R)  calculates the optimal feedback gain matrix K
%	such that the feedback law:
%
%		u = -Kx
%
%	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:
%					  -1
%		0 = S - A'SA + A'SB(R+B'SB) BS'A - Q

%	J.N. Little 4-21-85
%	Revised 6-23-86 JNL
%	Copyright (c) 1985, 1986 by the MathWorks, Inc.

error(nargchk(4,4,nargin));
error(abcdchk(a,b));

[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

% Check if q is positive semi-definite and symmetric
if any(eig(q) < -eps) | (norm(q'-q,1)/norm(q,1) > eps)
	error('Q must be symmetric and positive semi-definite')
end
% Check if r is positive definite and symmetric
if any(eig(r) <= -eps) | (norm(r'-r,1)/norm(r,1) > eps)
	error('R must be 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);
[d,index] = sort(abs(d));	 % sort on magnitude of eigenvalues
if (~((d(n) < 1) & (d(n+1)>1)))
	error('Can''t order eigenvalues, (A,B) may be uncontrollable.')
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;
