function [G1,G2,At,Bt,Dt,Rr]=dlqdu_pi(A,B,D,Q,Rw);
% DLQDU_PI syntax
% [G1,G2,At,Bt,Dt]=dlqdu_pi(A,B,D,Q,R);
% Purpose
% Compute LQ-optimal control feedback matrices G1 and G2 for
% the controller
% u=u+G1*(x-x_old)+G2*(y_old-r);
% On input
% A,B,D - discrete state space model matrices.
% Q - Wheigting matrix for the output vector y_k.
% R - Weighting matrix for the control deviation vector, Delta u_k=u_k-u_(k-1).
% On output
% G1 and G2 - Matrices in LQ optimal controller
% At, Bt, Dt  - Matrices in augmented state space model


%% Make augmented state space model for LQ-design.
nx=size(A,1); nu=size(B,2); ny=size(D,1);
At=[A,zeros(nx,ny);D,eye(ny,ny)];Bt=[B;zeros(ny,nu)];Dt=[D,eye(ny,ny)];
Qt=Dt'*Q*Dt;

%% Solve Riccati-equation and compute feedback matrix.
[K,Rr]=dlqr(At,Bt,Qt,Rw);
G=-K; 
G1=G(:,1:nx); G2=G(:,nx+1:nx+ny);
