%% Robust performance, Matlab SISO case study, 5/(s+1)^2: mu-synthesis
% © 2022, Antonio Sala Piqueras, Universitat Politècnica de València. All rights reserved.
% This code ran with no errors in Matlab R2022a
% Presentations (video):
% http://personales.upv.es/asala/YT/V/cerp4muEN.html , http://personales.upv.es/asala/YT/V/cerp5muEN.html .
% Objectives: Understand the ideas behind the theory of robust performance and scaled small gain, with a second-order single-variable example, executed with \mu-synthesis (looking for multipliers in an iterative automated way) instead of looking for multipliers "by hand" as in previous videos that were only of didactic/illustrative interest, but not advised as a go-to option in actual control design problems.
% Table of Contents
% Plant Model
%     Target robust performance bound (in frecuency domain)
% Mu-Synthesis design for robust performance
%         Theoretical goal
%         Generalized Plant (2x2) for musyn
%         Generalized Plant 3x3 from 2x2 + uncertainty inside (for comparison, this is NOT necessary)
%     Weighted generalised plant (2x2) for musyn
%     Mu-synthesis
%     Reduced-order controller
%         Controller order reduction
%         Direct MUSYN optimization of a PID regulator
% Time and frequency simulation
%
%% Plant Model

s=tf('s');
G=5/(s+1)^2; %model, NOMINAL

BoundDelta=0.5; %Unstructured additive uncertainty size
Greal=G+BoundDelta*ultidyn("DeltaNormalized"); %uncertain model for mu-síntesis

Greal_simul=G+BoundDelta*ultidyn("DeltaNormalized")*1/(0.03*s+1);
%we kill things from 33 rad/s onwards in simulations.

step(Greal_simul,G,12), grid on
bodemag(Greal_simul,logspace(-1,2)), grid on, hold on
bodemag(G,logspace(-1,2)), hold off, legend("Delta*LowPass", "Nominal",Location="best")

%
%% Target robust performance bound (in frecuency domain)

TargetBandwidth=1.01; %approx. inverse prop. to rise time
templateErr=makeweight(0.01,TargetBandwidth,1.8);
bodemag(templateErr,tf(1)), grid on
title("Ref->Error Bode Diagram maximum modulus, template")
legend("desired maximum template","0 dB (100% error)",Location='best')

% Note: for simplicity, we do not limit the control action by "performance" reasons (saturation, etc.), although in applications it would be recommended. In any case, the robustness in the face of additive uncertainty is achieved by limiting "u", so, actually, we are implicitly limiting "u" somehow.
%
%% Mu-Synthesis design for robust performance
%% Theoretical goal
% We would try to build a generalized 3x3 plant with uncertainty to apply (for example) scaled small gain theorem if we wanted to run the multiplier search on our own (as "manually" done in other previous videos).
%
% If, however, we build with an uncertain model only the 2x2 part that refers to groups 2 and 3, Matlab's Robust Control Toolbox takes care of extracting the uncertainty and (internally) setting up the 3x3 plant. [When I say 2x2 or 3x3 I mean "groups" of signals, each group has different dimensions, depending on each specific problem to be solved].
% *If we want to "explicitly" see the 3x3 plant that musyn uses, we can do it with the lftdata command (see below), but this explicit use is not necessary in most simple cases (musyn already takes care of it), and we'll pose the generalized plant with the uncertainty "inside".
%% Generalized Plant (2x2) for musyn
%
% We will write [err;err]=[1 -Greal;1 -Greal]*[ref;u]  avoiding multi-incidence of Greal:

PGenMu=minreal([[1;1] [-1;-1]*Greal] )%if we execute "ss" then uncertainty is destroyed. Matlab does it in state-space form anyway.

%% Generalized Plant 3x3 from 2x2 + uncertainty inside (for comparison, this is NOT necessary)
% We extract the uncertainty with lftdata as follows (it is really NOT necessary to do this; it is already done, internally, by musyn, transparent to us):

[M,Delta]=lftdata(PGenMu);
size(M)
Delta
zpk(M)

% It coincides with what we had proposed when searching "by hand" for the multiplier in other earlier videos. Indeed:
%

PGenOtherVideo=minreal(ss([0 0 1;-1 1 -G;-1 1 -G])); %NOT NEEDED when using musyn
zpk(PGenOtherVideo*blkdiag(BoundDelta,1,1)) %We add input weight with size of uncertainty %NOT NEEDED when using musyn

%% Weighted generalised plant (2x2) for musyn

Wref=1;
Win=blkdiag(Wref,1);
Wout=minreal(blkdiag(1/templateErr,1));
PGenPond=Wout*PGenMu*Win %uncertainty is inside PGenMu

%% Mu-synthesis
% Automated hinfsyn plus scaled-small-gain multiplier search (iterative) is done with:

tic, [Kmu,GAM]=musyn(PGenPond,1,1); toc, GAM %we do not explicitly use "M" "Delta" from lftdata, musyn does that internally

% *The scaling for an ultidyn SISO is a dynamic one, D(s), which improves the performance GAM that can be proven (with respect to a constant multiplier seen in other preliminary videos). See D fit, Fit Order.
% It would be a "theoretically better" option if the uncertainty is linear and time invariant... but notice how the dynamic multiplier raises the order of the resulting regulator. However, it would not prove robustness against non-linearity with harmonic distortion or variation in time.

size(Kmu) %lti uncertainty permite pesos en frecuencia, por eso sube el orden.

%% Reduced-order controller
% There are several options:
% reduce the order of the controller obtained with musyn,
% Tell musyn with some options to reduce the "fit order" of the  "D" scaling
% Directly optimize a tunable fixed-structure controller of lower order (such as a PID)
% We'll discuss first and third options.
%% Controller order reduction

hsvd(Kmu) %many small singular values is good news
Kmu_reduced=balred(Kmu,3);
wcgain(lft(PGenPond,Kmu_reduced))
zpk(Kmu_reduced)

% There is a "very fast" pole we will remove with freqsep, to see what we can get:

[Kslow,Kfast]=freqsep(Kmu_reduced,150);
zpk(Kslow)
zpk(Kfast)
Kmu_reduced2=Kslow+dcgain(Kfast);
wcgain(lft(PGenPond,Kmu_reduced2)) %we still have robust performance
zpk(Kmu_reduced2) %it's a PID+'noise filter' controller

% The resulting controller closely resembles a PID with a noise filter... this suggests direct optimization of a PID, which we'll see below.
%
%% Direct MUSYN optimization of a PID regulator

PID=tunablePID('PIDtest','PID');
ClosedLoopWithPID=lft(PGenPond,PID);tic
[TunedCL,GAM]=musyn(ClosedLoopWithPID); GAM,toc

% Well, we haven't lost practically anything with respect to the generic "state space" regulator. Well, it would still be necessary to reduce the bandwidth a bit to be "below" 1 as conceptually required, but 0.001 excess norm is not going to matter in practice (the internal tolerance of the H-infinity solvers by default is already 1 percent)...

TunedPID=pid(TunedCL.Blocks.PIDtest)
zpk(TunedPID) %quite similar to Kmu_reduced2 above, they achieve basically the same, apart from numerical tolerance stuff

%
%% Time and frequency simulation

Kfinal=Kmu_reduced2;
%Kfinal=TunedPID;
step(feedback(Greal*Kfinal,1),feedback(Greal_simul*Kfinal,1),8), grid on
legend(" Delta 0.5"," Delta 0.5*LowPass")
bodemag(feedback(1,Greal*Kfinal),feedback(1,Greal_simul*Kfinal),templateErr,logspace(-2.5,2.5)), grid on
h_line=findobj(gcf,'type','line');
h_line(46).LineWidth=4; %trial and error to get the right line to make thicker
legend("Delta 0.5","Delta 0.5*LowPass","cota máx. PR",Location="best") %error

% \bulletControl action? Any "final" control design must have a "sensible" control action. We have not limited it theoretically, but it is important in practice, so let's see what it is:

step(feedback(Kfinal,Greal_simul),6), grid on
title("ref->u time response (step)")
bodemag(feedback(Kfinal,Greal_simul),tf(1/BoundDelta),logspace(-2.5,2.5))
grid on, title("ref->u frequency response"),legend("Closed Loop","Small gain bound (robust stability)")

% Of course, it is below the robust stability bound (non-scaled small gain theorem), as robust performance is more stringent than robust stability. [robust stability requires the NOMINAL K/(1+GK) being below the 1/\|Delta_{add}\| bound].