%% Example 8.3 from Burl
% Vertically oriented single link robotic arm

%% Model
A = [0 1; 0.5 0];
Bu = [0; 0.1];
Bw = [0; 0.1];
C = [1 0];
D = 0;

G = ss(A,[Bu,Bw],C,D);

%% Control and estimation parameters
Sw = 1;
Sv = 10;

Q = [1 0;0 0];
R = 16;

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

Klqg  = lqgreg(F,K); 

%% Computing and plotting the stability margins
L = -Klqg*G(:,1);       % open loop transfer function, omit w as the input
figure(1)
margin(L)    

%% LTR design
Bwwf = [Bu, Bw, Bu];    % combined input matrix for the disturbance and fict. noise
Gwwf = ss(A,Bwwf,C,D);

Swf = 100000;           % spectral density of the fictitious noise
Swwf = [Sw, 0; 0 Swf];

[Fltr,L,P] = kalman(Gwwf,Swwf,Sv);

Kltr  = lqgreg(Fltr,K); 

Lltr = -Kltr*G(:,1);    % open loop transfer function, omit w as the input
figure(2)
margin(Lltr)