%% Stability margins and worst-case gains (2nd order open-loop example): usage of robust control toolbox commands
% © 2023, Antonio Sala Piqueras, Universitat Politècnica de València. All rights reserved.
% This code executed without errors in Matlab R2022b
% Video-presentation at: http://personales.upv.es/asala/YT/V/stabmrgEN.html
% Whole collection at: http://personales.upv.es/asala/YT/
% Objectives: understand the usage of robstab, wcgain and robgain in a simple uncertain 2nd order system (may be understood as a mass-spring-damper with uncertain damping).
%
%% Model
% System model will be y(s)=\frac{5}{s^2+(1+0.5\delta)\cdot s+5}\cdot u(s), with \delta\in\mathbb R, actually with -1<\delta<1, so we already normalised the damping uncertainty. Parametric uncertainty is coded as:

delt=ureal("Delta",0);

% Given that the transfer function \frac{a}{s^2+bs+c}  can be modelled in state-space as
% A=[0 1;-c -b],      B=[0;a],       C=[1 0],        D=0
% we may understand the model as the uncertain state-space \dot x=Ax+Bu, y=Cx+Du with A, B, C, D given as:

A=[0 1;-5 -(1+0.5*delt)]; %matrix with uncertain elements
%This uncertain matrix syntax would not work if "delt" were ultidyn
%modelling, for instance, an uncertain damper with dynamics inside.
% In such a case, we would need some system interconnection commands
% such as LFT, or a block-diagram representation with connect, etc.
% This is discussed in other modelling examples of my teaching materials.
GsUnc=ss(A,[0;5],[1 0],0);
GsUnc.InputName={'u'};
GsUnc.OutputName={'position'}
step(GsUnc), grid on

% Damping uncertainty translates to settling time and overshoot uncertainty, for instance.
%% Nominal model versus worst-case models

G_nominal=GsUnc.NominalValue;
zpk(G_nominal) %this is not "uncertain"

% Nominal resonance peak will be the "infinity norm":

[nominal_peakgain,freq]=norm(G_nominal,inf)

%% Robustness analysis (uncertain model)

[rbm,wcus]=robstab(GsUnc)
G_unstable=usubs(GsUnc,wcus);tf(G_unstable)

[wcg,wcug]=wcgain(GsUnc)
G_worstcaseperf=usubs(GsUnc,wcug); tf(G_worstcaseperf)

limgain=2.8;
[rbg,wcr]=robgain(GsUnc,limgain) %maximum "delta" to avoid hitting the peak gain limit
G_limgain=usubs(GsUnc,wcr); tf(G_limgain)

%%
%% Bode magnitude plots to check results

bodemag(G_nominal,G_worstcaseperf,G_limgain,logspace(-1,1,150)), grid on
yline(20*log10(wcg.UpperBound),'-.r',Label="wcgain")
yline(20*log10(nominal_peakgain),'-.b',Label="nominal, norm")
yline(20*log10(limgain),'-.g',Label="2.8, rogbain")
legend("Nominal","Worst Case","Gain Limit 2.8", Location="best")
%hold on
%bodemag(GsUnc) 
%hold off

%%