%% Example: LQG design for a satellite tracking antenna
% Ex. 8.1 from J.B.Burl: Linear optimal control, Addison-Wesley, 1999.

%% State-space model for a satellite tracking antenna
A = [0 1; 0 -0.1];          % matrices defining the model of the system
Bu = [0; 0.001];
Bw = [0; 0.001];
C = [1 0];
D = 0;

G = ss(A,[Bu,Bw],C,D)       % model without disturbance and noise
G.InputName = {'u','w'}
G.StateName = {'theta','omega'}
G.OutputName = 'theta'

%% Parameters for LQR and Kalman filter design 
Q = [18000 0; 0 0];           % weighting matrices for the LQR design
R = 1;

Sw = 5000;                  % spectral density [N^2 m^2 / Hz]
Sv = 1;                     % noise spectral density [deg^2/Hz]

%% LQR, Kalman filter and LQG design
[K,S,e] = lqr(A,Bu,Q,R);    % LQ-optimal state feedback
[F,L,P] = kalman(G,Sw,Sv);  % Kalman filter

Klqg  = lqgreg(F,K);        % LQG-optimal output feedback controller

%% Simulation in Simulink
tc = 0.001;                 % correlation time
piw = tc*Sw;                % noise power
piv = tc*Sv; 

%% Robustness
L = -Klqg*G(:,1);           % open loop transfer function, omit w as the input
margin(L)