%% Plant definition
% refer to to Sinha, FIGURE 5.1.1
clear;

k1 = ureal('k1', 100, 'Percentage', 20);
k2 = ureal('k2', 500, 'Percentage', 20);
m = ureal('m', 1, 'Percentage', 1);
alpha = ureal('alpha', 1, 'Percentage', 5);

A = [
    0 0 1 0;
    0 0 0 1;
    -(k1+k2)/m k2/m -alpha/m 0;
    k2/m -(k1+k2)/m 0 -alpha/m;
    ];

B = [0 0; 0 0; 1 0; 0 1];
C = [eye(2) zeros(2)];
D = zeros(2);

G = uss(A, B, C, D);
G_fullstate = uss(A, B, eye(4), zeros(4, 2));

% multiplicative uncertainty
w = logspace(0, 3, 40);
systems = usample(G, 40);
data = frd(systems, w)  ;
[~, Info] = ucover(data, G.NominalValue, 4, []);
Im = tf(Info.W1);

%% Controller Definition
Gn = G.NominalValue;
Gn_fullstate = G_fullstate.NominalValue;

s = tf('s');

% output sensitivity weight
Ms = 1.7; % peak sensitivity (~ stability margin)
omega_b = 5; % open loop bandwidth
eps_s = 0.01; % low frequency sensitivity upper bound (~ robust tracking)
Ws = (s/Ms + omega_b)/(s+omega_b*eps_s);

%%%% loop invertion controller
% K = inv(Gn);
% K = K*1/s * 1/(1+10*s) * (1+0.7*s) * 100;
% K.OutputName = 'u';
% 
% AP_u = AnalysisPoint('u', 2);
% Gcl = feedback(G*AP_u*K, eye(2));
% Gcl.InputName = 'r';
% Gcl.OutputName = 'y';
% L = Gn*K;

%%%% PI diagonal controller
% D = evalfr(inv(G), 0);
% K = D*(s^2 + s + 100) / (1+s) / (1+s/10);
% 
% Gcl = feedback(G*K, eye(2));
% L = Gn*K;

%%%% LQI controller
% state weight + integrators weight
Q = blkdiag(1e3*eye(4), 1e7*eye(2));
% input weight
R = 1e-3*eye(2);
K = lqi(Gn, Q, R, zeros(6, 2));
K = ss(K);
K.InputName = {'x(1)', 'x(2)', 'x(3)', 'x(4)', 'e(1)', 'e(2)'};
K.OutputName = 'u';
G_fullstate.InputName = 'u';
G_fullstate.OutputName = 'x';
G_feedback = connect(G_fullstate, -K, 'e', 'x', {'u', 'x'});
% only select first and second output
G_feedback = G_feedback([1 2], [1 2]);
G_feedback.OutputName = 'y';

Gcl = feedback(G_feedback*1/s, eye(2));
Gcl.InputName = 'r';
L = ss(G_feedback)*1/s;

%%%% Sensitivity functions
S = inv(eye(2)+L);
T = eye(2) - S;

if ~isstable(T)
    disp('Nominal closed loop is not stable');
else
    disp('Nominal closed loop is stable');
end

if hinfnorm(S*Ws) <= 1
    disp('Nominal performance specifications on sensitivity are met')
else
    disp('Nominal performance specifications on sensitivity are not met')
end
%% close figures
close all;

%% Singular value robustness and performance analysis
figure('Name', 'Robust stability analysis');
w = logspace(-1, 10, 100);
SV = sigma(eye(2) + inv(L), w);
semilogx(w, 20*log10(min(SV, [], 1))); hold on;
sigma(Im, w, 'r'); hold off;
title('Robust stability analysis')

figure('Name', 'Sensitivity and loop performance');
% open loop performance specifications
subplot(2, 2, 1);
sigma(L);
title('Open loop specifications: L');

subplot(2, 2, 2);
[SV, W] = sigma(S);
semilogx(W, 20*log10(max(SV, [], 1))); hold on;
% sensitivity weight
r = freqresp(1/Ws, W);
semilogx(W, 20*log10(abs(r(:, :))), 'r');
hold off;
title('Open loop specifications: S');

subplot(2, 2, 3);
[SV, W] = sigma(T);
semilogx(W, 20*log10(max(SV, [], 1)));
title('Open loop specifications: T');

%% Structured singular value analysis
opts = robOptions('VaryFrequency','on');
[stabmarg,wcg,info] = robstab(Gcl,opts);
figure('Name', 'mu analysis');
semilogx(info.Frequency,info.Bounds)
title('Stability Margin vs. Frequency');
ylabel('Margin');
xlabel('Frequency');
legend('Lower bound','Upper bound');

if stabmarg.LowerBound > 1
    fprintf('Closed loop system is robustly stable: mu=%f-%f at %f rad/s\n', ...
        stabmarg.LowerBound, stabmarg.UpperBound, stabmarg.CriticalFrequency);
else
    fprintf('Closed loop system is not robustly stable: mu=%f-%f at %f rad/s\n', ...
        stabmarg.LowerBound, stabmarg.UpperBound, stabmarg.CriticalFrequency);
end

%% Plant sv and multiplicative uncertainty
figure('Name', 'Open Loop System');
subplot(2, 1, 1);
sigma(G);
hold on;
title('Singular values of the system');

subplot(2, 1, 2);
sigma(Im);
title('Multiplicative Uncertainty Magnitude');

%% Simulation
t = 0:0.001:3;
r1 = 0*t + 0.1; % 10 centimeters displacement
r2 = 0*t;

% transfer function from reference to controller output
G_ur = getIOTransfer(Gcl, 'r', 'u');

y = lsim(T, [r1; r2], t);
u = lsim(G_ur, [r1; r2], t);
realizations = usample(Gcl, 20);
ninputs = 2;
y_unc = zeros(ninputs*length(realizations), length(t));
u_unc = zeros(ninputs*length(realizations), length(t));

for i=1:length(realizations)
    G_ur_i = getIOTransfer(realizations(:, :, i, 1), 'r', 'u');
    p = lsim(realizations(:,:, i, 1), [r1; r2], t);
    q = lsim(G_ur_i, [r1; r2], t);
    y_unc(ninputs*i, :) = p(:, 1);
    y_unc(ninputs*i+1, :) = p(:, 2);
    u_unc(ninputs*i, :) = q(:, 1);
    u_unc(ninputs*i+1, :) = q(:, 2);
end

Twc = usubs(Gcl, wcg);
Tur = usubs(G_ur, wcg);
y_wc = lsim(Twc, [r1; r2], t);
u_wc = lsim(Tur, [r1; r2], t);

%% Output and control action plot
figure('Name', 'Transient simulation: output');
subplot(2, 3, 1);
plot(t, r1, '--b', t, y(:, 1), 'r');
title('Nominal system output');
subplot(2, 3, 4);
plot(t, r2, '--b', t, y(:, 2), 'r');

subplot(2, 3, 2);
plot(t, r1, '--b'); hold on;
for i=1:length(realizations)
    plot(t, y_unc(ninputs*i, :), 'r');
end
hold off;
title('Uncertain system output');

subplot(2, 3, 5);
plot(t, r2, '--b'); hold on;
for i=1:length(realizations)
    plot(t, y_unc(ninputs*i+1, :), 'r');
end
hold off;

subplot(2, 3, 3);
plot(t, r1, '--b', t, y_wc(:, 1), 'r');
title('Worst case system output');

subplot(2, 3, 6);
plot(t, r2, '--b', t, y_wc(:, 2), 'r');

figure('Name', 'Transient simulation: control');
subplot(2, 3, 1);
plot(t, r1, '--b', t, u(:, 1), 'r');
title('Nominal system control action');
subplot(2, 3, 4);
plot(t, r2, '--b', t, u(:, 2), 'r');

subplot(2, 3, 2);
plot(t, r1, '--b'); hold on;
for i=1:length(realizations)
    plot(t, u_unc(ninputs*i, :), 'r');
end
hold off;
title('Uncertain system control action');

subplot(2, 3, 5);
plot(t, r2, '--b'); hold on;
for i=1:length(realizations)
    plot(t, u_unc(ninputs*i+1, :), 'r');
end
hold off;

subplot(2, 3, 3);
plot(t, r1, '--b', t, u_wc(:, 1), 'r');
title('Worst case system control action');

subplot(2, 3, 6);
plot(t, r2, '--b', t, u_wc(:, 2), 'r');