%  Example 8.5.  LQG tracking system design (feedforward and integral control).
clear
%  Define the plant model.
A=[ 0   1
   -10 -1];
Bu=[0 1]';
Cy=[1 0];
%
%  Tracking system design via control bias.
%  Compute the state feedback controller gain
K=lqr(A,Bu,[1 0;0 0],0.001)
%  Compute the control bias via a gain on the reference input.
Kr=-1/(Cy*inv(A-Bu*K)*Bu)
%  Simulate the control system.
t=0:0.02:2;
r=ones(size(t));
[y,x]=lsim(A-Bu*K,Bu,Cy,0,Kr*r',t);
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(211),plot(t,y)
xlabel('Time (sec)')
ylabel('Flapper angle')
grid
subplot(212),plot(t,-K*x'+Kr*r)
xlabel('Time (sec)')
ylabel('Control torque')
grid
%
%  Tracking system design using integral control.
%  Generate the augmented plant.
Aa=[ A  zeros(2,1)
    -Cy     0     ];
Ba=[Bu
    0 ];
Br=[0 0 1]';
Ca=[Cy 0];
%  Define the augmented cost function parameters.
Qa=zeros(3);Qa(3,3)=1;
Ra=0.0001;
%  Compute the integral LQR gain.
Ka=lqr(Aa,Ba,Qa,Ra)
%  Simulate the control system.
t=0:0.02:3;
r=ones(size(t));
ya=lsim(Aa-Ba*Ka,Br,[Ca;-Ka],[0;0],r',t);
%  Plot the results.
figure(2)
clf
subplot(211),plot(t,ya(:,1))
xlabel('Time (sec)')
ylabel('Flapper angle')
grid
subplot(212),plot(t,ya(:,2))
xlabel('Time (sec)')
ylabel('Control torque')
grid
