%% LQ-optimal tracker with an integral component
% Ex. 8.5 from Burl
% Create an LTI state-space model

A = [0 1; -10 -1];
B = [0; 1];
C = [1 0];
D = 0;
nx = 2;
nu = 1;
ny = 1;
A = rand(nx,nx);
B = rand(nx,nu);
C = rand(ny,nx);
G = ss(A,B,C,D)

% Set the Q and R weights

Q = zeros(nx+ny,nx+ny); Q(3,3) = 1

% Note that we are only penalizing the tracking error.

R = 0.0001;

%
% Design of an LQR for an augmented system. But no need to create an augmented system, because a wrapper already available as lqi:

[Kaug,S,E] = lqi(G,Q,R)

% The state feedback component

K = Kaug(1:nx)

% and the component corresponding to the tracking error

Ki = Kaug(nx+1:end)

%
% Simulation

Z1 = zeros(nx,nu);
Z2 = zeros(ny,nu);
Aaug = [A-B*K -B*Ki;-C 0];
Baug = [Z1; eye(ny, nu)];
Caug = [C 0]; %eye(nx+ny, nx+ny);
Gaug = ss(Aaug,Baug,Caug,0);
step(Gaug)