%  Ex10_6LQG.m - Generates the LQG/LTR autopilot.
clear
%  Define the plant model.
Za=-5.24;Zd=-0.4;Ma=-47;Mtd=-4.69;Md=-1134;V=1.82;d=8.55e-4;
wn=189;z=0.7;
A=[   Za    1      Zd      0
      Ma   Mtd     Md      0
      0     0      0       1
      0     0   -wn*wn  -2*z*wn];
Bw=[1e3 0    0
     0 1e3   0
     0  0    0
     0  0  wn*wn];
Bu=[  0
      0
	  0
	wn*wn];
Cy=[V*Za 0 V*Zd  0
     0   0  1    0
     0   0  0    1];
Cm=[V*Za+Ma*d Mtd*d V*Zd+Md*d 0
        0       1       0     0];
%
%  Generate integral LQR gains.
%
%  Generate an augmented plant model.
Aa=[   A     zeros(4,1)
    Cy(1,:)      0     ];
Ba=[Bu;0];
%  Define the cost function for the augmented plant.
Qy=diag([0 1.4 0.1]);
Qa=[Cy'*Qy*Cy  zeros(4,1)
    zeros(1,4)     50   ];
Ra=1;
%  Compute the integral LQR gains.
Ka=lqr(Aa,Ba,Qa,Ra);
Klqr=Ka(1:4);Ki=Ka(5);
%
%  Generate the Kalman filter.
%  Define the noise spectral densities.
Sw=diag([1 1 1]);
Sv=diag([1e-6 1e-3]);
%  Compute the Kalman gain.
Gkal=lqe(A,Bw,Cm,Sw,Sv);
%
%  Generate the state model of the controller.
Ac=[A-Gkal*Cm-Bu*Klqr -Bu*Ki
         Cy(1,:)         0  ];
Bc=[   Gkal    zeros(4,1)
    zeros(1,2)     -1    ];
Cc=-Ka;
%  Generate a system matrix K containing the controller.
K=pck(Ac,Bc,Cc,[0 0 0]);
