%  Example 8.6.  LQG control for disturbance rejection (feedforward and 
%    integral control).
clear
%  Define the plant model.
A=[-1 1;0 -0.1];
Bu=[0 0.1]'; Bw0=[1 0]'; Bw=[0 1]';
Cm=[1 0]; Cy=[1 0];
%  Define the cost function parameters.
Q=diag([1 0]); R=0.001;
%  Generate an LQR controller and use -Kx+Kww for feedback.
'Gains for Feedforward Disturbance Rejection'
K=lqr(A,Bu,Q,R)
Kw=-(Cy*inv(A-Bu*K)*Bu)\(Cy*inv(A-Bu*K)*Bw0)
%  Simulate the system with a step disturbance input.
t=0:.01:20;
[y,x]=step(A-Bu*K,Bu*Kw+Bw0,[Cy;-K],[0;Kw],1,t);
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(211),plot(t,y(:,1))
xlabel('Time (sec)')
ylabel('Angular velocity (rad/sec)')
grid
subplot(212),plot(t,y(:,2))
xlabel('Time (sec)')
ylabel('Field voltage (volts)')
grid
%
%  Generate an LQG controller with integral control.
%
%  Generate the augmented plant.
Aa=[ A  zeros(2,1)
    -Cy     0     ];
Ba=[Bu;0];
%  Define the augmented cost function parameters.
Qa=diag([0 0 1]); Ra=0.0001;
%  Compute the integral LQR gain.
Ka=lqr(Aa,Ba,Qa,Ra);
K=Ka(1:2);Ki=Ka(3);
%
%  Generate Kalman filter (with integral gain) gain.
%
%  Generate the augmented plant.
Af=[    A      Bw0
    zeros(1,2)  0 ];
Bwf=[Bw zeros(2,1)
     0      1     ];
Buf=[Bu
     0 ];
Cmf=[Cm 0];
%  Define the spectral densities for the augmented plant.
Sw=0.001;
Swf=[Sw    0
     0   0.01];
Sv=0.001;
%  Compute the integral Kalman gain.
Gf=lqe(Af,Bwf,Cmf,Swf,Sv);
G=Gf(1:2);Gi=Gf(3);
%  Compute the state model of the controller.
Ac=[Af-Buf*[K 0]-Gf*Cmf -Buf*Ki
	    -[Cy 0]             0  ];
Bc=[Gf; 0];
Cc=-[K 0 Ki];
%  Generate the closed loop state model.
Acl=[  A   Bu*Cc
     Bc*Cm  Ac  ];
Bcl=[    Bw     zeros(2,1)    Bw0
     zeros(4,1)     Bc     zeros(4,1)];
Ccl=[    Cy     zeros(1,4)
     zeros(1,2)     Cc    ];
Dcl=zeros(2,3);
%  Generate the input for simulation.
lt=length(t);
input=[100*Sw*randn(lt,1) 100*Sv*randn(lt,1) ones(lt,1)];
%  Simulate the control system.
[y,x]=lsim(Acl,Bcl,Ccl,Dcl,input,t);
%  Plot the results.
figure(2)
clf
subplot(211),plot(t,y(:,1))
xlabel('Time (sec)')
ylabel('Angular velocity (rad/sec)')
grid
subplot(212),plot(t,y(:,2))
xlabel('Time (sec)')
ylabel('Field voltage (volts)')
grid
