function [f,p,msg] = dlqrdr(beta,A11,A12,A13,A22,A23,B1,Q,R,W,C1,C2,C3);
%DLQRD   Linear quadratic regulator design for discrete-time systems
%        with side constraints.
%        [f,p] = dlqrdr(beta,A11,A12,A13,A22,A23,B1,Q,R,W,C1,C2,C3)
%	 recursively calculates the optimal feedback gain matrix F 
%        such that the feedback law:
%
%        (*)    u = -Fx = -F1*x1 - F2*x2 
%
%        minimizes the cost function:
%
%		J = Sum beta^n{x[n]'Qx[n] + u[n]'Ru[n] + 2x[n]'Wu[n]}
%
%        subject to the constraint equations: 
%		
%	        x1[n+1] = A11*x1[n] + A12*x2[n] + A13*x3[n] + B1*u[n]
%	        x2[n+1] =             A22*x2[n] + A23*x3[n] 
%
%        with x=[x1;x2;x3] and {x3[n]} given.  The solution (*) 
%        is found by imposing the following side constraints (e.g.
%        market clearing conditions) on first-order conditions
%
%               x3[n] = C1*x1[n] + C2*x2[n] + C3*u[n].
%
%	 Also returned is P, the steady-state solution to the associated 
%	 discrete matrix Riccati equation.
%        See also DLQRD. 

%	 Ellen McGrattan 1-22-89
%	 Revised 6-9-91, ERM

[n1,k]=size(B1);
[n2,n3]=size(A23);
n  = n1+n2;
B  = [B1;zeros(n2,k)];
A  = sqrt(beta)*([A11,A12,A13;zeros(n2,n1),A22,A23]-B/R*W');
A3 = A(:,n+1:n+n3);
A  = A(:,1:n);
W3 = W(n+1:n+n3,:);
B  = sqrt(beta)*B;
Q  = Q-W/R*W';
Q3 = Q(1:n,n+1:n+n3);
Q  = Q(1:n,1:n);
W  = W(1:n,:);

%    x3[t]=a1*[x1[t];x2[t]]+a2*mu[t+1]
c  = [C1,C2];
a1 = c-C3*((R+W3'*C3)\(W'+W3'*c));
a2 = -C3*((R+W3'*C3)\B');
b  = B+A3*C3/(R+W3'*C3)*R;
q  = Q+Q3*a1;
a1 = A+A3*a1;
a2 = A'+Q3*a2;

p  = -eye(n);
f0 = (R+B'*p*b)\B'*p*a1;
dd = 1;
it = 1;
maxit=5000;
tol=1e-6;       % reset tolerance if necessary 
while (dd> tol & it<= maxit);
  p  = q+a2*p*(a1-b*f0);
  f1 = (R+B'*p*b)\B'*p*a1;
  dd = norm(f1-f0); 
  it = it+1;
  f0 = f1;
end;
if it>=maxit; 
  sprintf('WARNING: Iteration limit of %g reached in DLQRDR',maxit)
end;
f = (R+W3'*C3)\(R*f1+W'+W3'*c);
