function [G1,G2,R]=dlq_du(A,B,D,Q,P)
% PURPOSE
% Compute LQ optimal controller with integral action as presented 
% inpaper by Di Ruscio.
%
% u_k=u_(k-1) + G1*(x_k-x_(k-1)) + G2*(y_(k-1)-r)
%
% where, r, is the reference

nx=size(A,1);
nu=size(B,2);
ny=size(D,1);

At=[A zeros(nx,ny); D eye(ny)]; 
Bt=[B; zeros(ny,ny)];    
Dt=[D eye(ny)];
Qt=Dt'*Q*Dt;

[G,R]=dlqr(At,Bt,Qt,P); G=-G;
G1=G(:,1:nx); G2=G(:,nx+1:nx+ny);
