%% Case study: actuator (manipulated variables) and controlled variable selection, polyhedra vs SVD tools
% © 2024, Antonio Sala Piqueras, Universitat Politecnica de Valencia, SPAIN. All rights reserved.
% Presentations in video:
% http://personales.upv.es/asala/YT/V/sacerf1EN.html
% http://personales.upv.es/asala/YT/V/sacerf2EN.html
% http://personales.upv.es/asala/YT/V/sacerf3EN.html
% http://personales.upv.es/asala/YT/V/sacerf4EN.html
% http://personales.upv.es/asala/YT/V/sacerf5EN.html
% http://personales.upv.es/asala/YT/V/sacerf6EN.html
% http://personales.upv.es/asala/YT/V/sacerf7EN.html
% This code runs in Matlab R2023a (Linux)
% Objectives: understand SVD and polyhedron geometry and manipulations to asses wheter given setpoint increments or worst-case disturbance rejection are feasible without saturation.
%
% Table of Contents
% Model and constraints
% A) Reference tracking in steady state without saturation, SVD geometry
%         Basic computations
%         Further discussion
% B) Steady state reference tracking, Polyhedron geometry
%         Basic computations
%         Further discussion
% C) Total disturbance rejection without saturation (steady state)
%         SVD geometry
%         Polyhedron geometry, total cancellation (steady state)
% D) partial disturbance rejection (steady state)
%     D.1) Polyhedron geometry
%     D.2) SVD (ellipse) geometry
% E) Transient analysis (bandwith), setpoint and disturbance
%         Combined setpoint + disturbances (simultaneously, but norm 1)
%
%% Model and constraints
% Consider a linearised model y_{2\times 1}=G_{2\times 3}u_{3\times 1}+H_{2\times 2}d_{2\times 1} with an operating point given by:

y_eq=[9 1.45]; u_eq=[2 0.5 1.7]; d_eq=[3 2];

% where G and H are the transfer function matrices:

s=tf('s');
G=[3/(s+2)  0.15/(2*s+1)    -9/(0.3*s+1);
    0.15/(0.5*s+1)  -0.2/(s+1)  1.9/(0.3*s+1)];
H=[1.8/(s+1)^2 2/(s+3); 2.4/(s+12) 0.6/(s+1)/(s+3)];

%
%
%
%
% Later on, we'll see that we can achieve all that we are required to... Hence, if we wish to test "actuator selection", using just two of them, we may make some columns of G equal to zero and execute the code again:

G=G*diag([1 1 1]); %set to zero position of actuator to disable.

% Of course, "elliminating" one actuator should be, in rigor, "deleting" the column, but that would change the size of matrices so code would give errors. Setting column to zero is a quick workaround.
% Manipulated variables u have the following saturation limits:

lim_u_abs=[4 1.5 2; 0 0 1]; %1st row max, 2nd row min

% In incremental units, the limits of manipulated variables are:

inc_u_admissible=lim_u_abs-u_eq

%%
%%
%% A) Reference tracking in steady state without saturation, SVD geometry
%% Basic computations
% We wish to be able to move the outputs (via setpoint changes to be tracked by controllers) in the ranges:

lim_y_abs=[11.6 1.6; 8.5 1.35]; %1st row max, 2nd row min

% So, in incremental units, these desired increments are:

inc_y_desired=lim_y_abs-y_eq

% Static DC gain matrix is:

Gain=dcgain(G)

% Scaling step: y=E_y\cdot y_{esc},    u=E_u\cdot u_{esc}

Ey=diag(max(abs(inc_y_desired))) %worst case is maximum desired output increment
Eu=diag(min(abs(inc_u_admissible))) %worst case is minimum available input increment

% The scaled gain matrix is given by:
% y_{esc}=E_y^{-1}y=E_y^{-1}Gu=\underbrace{E_y^{-1}GE_u}_{scaled\ gain}\cdot u_{esc}

Gain_scaled=inv(Ey)*Gain*Eu %Scaling for unit increments desired/available.
svd(Gain_scaled)

% Minimum gain is almost 1.5, above 1: satisfactory. Condition number is:

cond(Gain_scaled)

% The value of around 3 is quite sensible, lower than 5.
% Hence, the answer is YES: we can move the outputs to the required amplitudes (steady state) with available inputs, when considering the geometry of ellipses, i.e., achieving any y_{esc} such that \|y_{esc}\|=1 with \|u_{esc}\|\leq 1, norm is the Euclidean norm.
% In a "pen-and-paper plus basic calculator" examination for my M.Sc. students this would finish the required answer.
%
%% Further discussion
% We will now examine singular vectors (principal directions) for further insight:

[U,S,V]=svd(Gain_scaled)

% Intuitively, the "hard" manoeuver is, roughly, increase 1 unit output 1 and increase 0.1 units output 2 (grosso modo, keep it where it was), being manipulated variable 1 the most critical.
% Also, as minimum gain is above \sqrt{2}, then the "unit sphere in u" will be able to achieve all output manoeuvers in the sphere of radius 1.49 in y space, which of course will include the "unit square in y, vertices at \pm 1"; thus, the validity in the polyhedron geometry can be asserted without any polyhedron-specific code. This would not occur if the minimum gain were lower than  \sqrt{\text{num.\, outputs}}.
% Let us graphically represent the above ideas.
% If we draw the output ellipse swept by inputs \|u_{esc}\|=1 when multiplied by G_{esc}, we have:

M=Gain_scaled*Gain_scaled'
syms y1 y2 real
y=[y1;y2];
fill([-1 -1 1 1],[-1 1 1 -1],[.55 0.95 .8]), hold on %unit square
fimplicit(y'*inv(M)*y-1, LineWidth=3), grid on %feasible ellipse
fimplicit(y'*y-1, '-.',LineWidth=2) %unit circle
fimplicit(y'*y-S(2,2)^2, '-.',LineWidth=2) %mingain circle
plot([0 U(1,1)*S(1,1)],[0 U(2,1)*S(1,1)],LineWidth=2.5) %output direction 1, scaled
plot([0 U(1,2)*S(2,2)],[0 U(2,2)*S(2,2)],LineWidth=1.25) %output direction 2, scaled
for i=1:3
    quiver(0,0,Gain_scaled(1,i),Gain_scaled(2,i),0,Color=[.9 .6 .5],LineWidth=1.5);
end
hold off, axis equal
xlabel("y1esc"),ylabel("y2esc")
legend("Square \pm 1","Feasible outputs for ||u||<1 (incr. sc.)","Desired outputs (incr. sc)","Minumum gain","High gain princ. manoeuver","Low gain princ. manoeuver","Individual MV manoeuver", Location="best")

%
%% B) Steady state reference tracking, Polyhedron geometry
%% Basic computations
% With polyhedron code, there is no need for "scaled" units, and in fact as ranges are quite asymmetric, even in scaled units the desired increments will not be "unity".
% So, we'll work in original non-scaled units, but of course in INCREMENTAL ones, as we are working with a linear system. We have:

Vertices_incy=[8.5 8.5 11.6 11.6;1.35 1.6 1.6 1.35]-[9;1.45] %desired extreme points
Vertices_incy=Vertices_incy; %scaling to check for extra margin
Min_incU=inc_u_admissible(2,:) %Lower Bound
Max_incU=inc_u_admissible(1,:) %Upper Bound

% We must check if there is a feasible u for each of the four extreme vertices of desired output.
% The code below minimises \|u\|^2 subject to  Gan·u=vertex , and subject to u being inside the maximum and minimum increment bounds.

quadprog(inv(Eu)^2,zeros(1,3),[],[],Gain,Vertices_incy(:,1),Min_incU,Max_incU) %factible
quadprog(inv(Eu)^2,zeros(1,3),[],[],Gain,Vertices_incy(:,2),Min_incU,Max_incU) % NO factible
quadprog(inv(Eu)^2,zeros(1,3),[],[],Gain,Vertices_incy(:,3),Min_incU,Max_incU) % NO factible
quadprog(inv(Eu)^2,zeros(1,3),[],[],Gain,Vertices_incy(:,4),Min_incU,Max_incU) %factible

% As all vertices are feasible, we have proven that the requested steady state setpoint tracking problem is feasible without MV saturation, and we can achieve any point in the desired "output box". We knew it from the minimum-gain SVD computations, anyway.
%% Further discussion
% Let us graphically represent the result of the above computations.

inc_u_admissible
VerticesIncU=[2 1 .3;2 1 -.7;2 -.5 -.7;2 -.5 .3;-2 1 .3;-2 1 -.7;-2 -.5 -.7;-2 -.5 .3]'
ImageVerticesU=Gain*VerticesIncU
k=convhull(ImageVerticesU(1,:),ImageVerticesU(2,:)); %Order is important for "fill"
fill(ImageVerticesU(1,k),ImageVerticesU(2,k),'g') %feasible polyhedron
hold on
fill(Vertices_incy(1,:),Vertices_incy(2,:),'c')
hold off, grid on, axis equal
xlabel("y1"),ylabel("y2")
xline(0,'r'),yline(0,'r'), legend("Feasible Output Polyhedron","Desired Output Polyhedron")

% Even if it is not strictly needed, we can plot in "scaled" units, to compare with the ellipses, etc:

VerticesIncU_SquareWorstCase=[2 .5 .3;2 .5 -.3;2 -.5 -.3;2 -.5 .3;-2 .5 .3;-2 .5 -.3;-2 -.5 -.3;-2 -.5 .3]';
ImageVerticesUWC=Gain*VerticesIncU_SquareWorstCase;
fill(1/Ey(1,1)*ImageVerticesU(1,k),1/Ey(2,2)*ImageVerticesU(2,k),'g'), 
hold on
fill(1/Ey(1,1)*ImageVerticesUWC(1,k),1/Ey(2,2)*ImageVerticesUWC(2,k),[0.45 0.95 0.45],LineStyle=':') %feasible polyhedron "Worst Case"
fill([-1 -1 1 1],[-1 1 1 -1],[0.55 0.95 0.85],LineStyle=':'), hold on %unit square
fill(1/Ey(1,1)*Vertices_incy(1,:),1/Ey(2,2)*Vertices_incy(2,:),'c'), 
fimplicit(y'*inv(M)*y-1, LineWidth=3), grid on %feasible ellipse
fimplicit(y'*y-1, '-.',LineWidth=2) %unit circle
hold off, axis equal, xline(0,'r'),yline(0,'r')
xlabel("y1esc"),ylabel("y2esc")
legend("Feasible polyhedron (scaled)","Feas. with Worst-case Input","Worst-Case Unit square","Desired box (esc)","Feasible ellipse ||u||<1","Circle 1",Location="best")

% On scaling and relation to predictive control or LQR cost index
% If we wished to achieve a given setpoint minimising \|u\| in "online" operation, we would end up doing something VERY similar to the above quadprog. MPC would use not just the DC gain, but the whole step response (DMC). In a general case, if each element of u has its own units and range, we should actually minimise \|u_{esc}\|^2 so that all increments are "comparable". We may switch to  G_{esc} and scale output accordingly, or we may keep original units and minimise  \|u_{esc}\|^2=\|E_u^{-1}u\|^2, from the fact that, by definition, u=E_uu_{esc}. This would just require to change the first argument to quadprog from "eye(3)" to "inv(E_u)^2".
%
%% C) Total disturbance rejection without saturation (steady state)
%% SVD geometry
% With a model y=Gu+Hd, input to fully cancel the effect of disturbance is disturbance multiplied by the "feedforward gain matrix" -G^{-1}H o; in the case G is not square, we need pseudoinverse (scaled):

interv_d=[3.8 2.4;2.4 1.35]; %range (absolute units) of the two disturbances.
operatingpoint_d=[3 2];
incr_d=interv_d-operatingpoint_d %incremental units
Ed=diag(max(abs(incr_d))) %worst case is maximum disturbance
Hgesc=inv(Ey)*dcgain(H)*Ed;
%Actually, Ey is not relevant for total cancellation. Only Eu and Ed matter.
FeedForward=-pinv(Gain_scaled)*Hgesc
u_sv=svd(FeedForward)

% Maximum gain <1 means that scaled disturbances \|d_{esc}\|\leq 1 can be cancelled with \|u_{esc}\|\leq 0.552<1.
% Graphical representation

Md=Hgesc*Hgesc';
fimplicit(y'*inv(M)*y-1, LineWidth=2.5), grid on %feasible ellipse
hold on
%elipsoide que las entradas pueden mover
fimplicit(y'*inv(M)*y-u_sv(1)^2, '-.',LineWidth=1.5), grid on %feasible ellipse
fimplicit(y'*inv(Md)*y-1, LineWidth=2) %output ellipse in open loop due to disturbances
xlabel("y1esc"),ylabel("y2esc")
hold off, axis equal, legend("Reachable ellipse with ||u||=1","Reachable ellip. with ||u||=0.55","Reachable ellipse ||d||=1",Location="bestoutside")

% Indeed, we can see that the effect of u is larger than that of d, so u has no problems to counteract the "yellow" ellipsoid producing an output contrary to it inside the "red" ellipsoid.
%% Polyhedron geometry, total cancellation (steady state)

%zoomfactor=1;
zoomfactor=2.08 %for partial rejection, later on
%zoomfactor=1.78 %max. feasible for total rejection below
Vertices_incD=[0.8 0.8 -0.6 -.6; 0.4 -0.65 0.4 -0.65]*zoomfactor 
1/zoomfactor

% *Minimising \|u_{scaled}\|^2=u^T(E_u^{-1})^2u would make pseudo-inverse results coincident with quadprog ones (if pseudo-inverse were feasible). Note, however, that actual "implementation" would need a feedforward (measurable disturbance) component, both here and in the pseudo-inverse SVD computations.

for i=1:4
    i
    quadprog(inv(Eu^2),zeros(1,3),[],[],Gain,-dcgain(H)*Vertices_incD(:,i),Min_incU,Max_incU); %set equality Gu=-Hd_i
end

% So, we may plot the relevant polyhedra to check the meaning of the above. In Non-scaled coordinates:

ImageVerticesd=-dcgain(H)*Vertices_incD; %Change sign, because "u" must counteract the effect.
kd=convhull(ImageVerticesd(1,:),ImageVerticesd(2,:)); %Order is important for "fill"


fill(ImageVerticesU(1,k),ImageVerticesU(2,k),'g') %feasible polyhedron
hold on
fill(ImageVerticesd(1,kd),ImageVerticesd(2,kd),'c')
hold off, grid on, axis equal
xlabel("y1"),ylabel("y2")
xline(0,'r'),yline(0,'r'), legend("Reachable Output Polyhedron 'u'","- Reachable Polyhedron 'd'")

% Even if it is not strictly needed, we can plot in "scaled" output units, to compare with the ellipses, etc:

fill(1/Ey(1,1)*ImageVerticesU(1,k),1/Ey(2,2)*ImageVerticesU(2,k),'g'), %Reachable with u
hold on
%reachable with worst-case U in scaling: cube of vertices +/-1.
fill(1/Ey(1,1)*ImageVerticesUWC(1,k),1/Ey(2,2)*ImageVerticesUWC(2,k),[0.45 0.95 0.45],LineStyle=':') %feasible polyhedron "Worst Case"

%reachable with 'd' in worst-case unit scaled square
Vertices_incDSquare=[0.8 0.8 -0.8 -.8;0.65 -0.65 0.65 -0.65]; %worst-case "square" of disturbances
ImageVerticesdSquare=-dcgain(H)*Vertices_incDSquare; %Change sign, because "u" must counteract the effect.

fill(1/Ey(1,1)*ImageVerticesdSquare(1,kd),1/Ey(2,2)*ImageVerticesdSquare(2,kd),[0.45 0.95 0.85],LineStyle=':')
%reachable with non-symmetric rectangle in d
fill(1/Ey(1,1)*ImageVerticesd(1,kd),1/Ey(2,2)*ImageVerticesd(2,kd),'c'), 

fimplicit(y'*inv(M)*y-1, LineWidth=2.5), grid on %reachable ellipse 'u'
fimplicit(y'*inv(Md)*y-1, LineWidth=2) %output ellipse in open loop due to disturbances
hold off, axis equal, xline(0,'r'),yline(0,'r')
xlabel("y1esc"),ylabel("y2esc")
legend("Reachable polyhedron 'u'","Reachable polyhedron with cube |u|<1","- Reachable polyhedron square |d|<1","- Reachable polyh. 'd'","Reachable ellipse ||u||<1","Reachable ||d||<1",Location="best")

%
%% D) partial disturbance rejection (steady state)
%% D.1) Polyhedron geometry
% Now, we change to -\epsilon_{Low} \leq Gu+Hd \leq +\epsilon_{High} or, well, the bound we wish...
% Rewrite first inequality as -\epsilon_{Low}-Hd\leq Gu, -Gu\leq \epsilon_{Low}+Hd and second one as Gu\leq \epsilon_{High} -Hd. Even the bound may be "vertex dependent" (left to the reader/viewer).
% For instance, if we want residual error in [-.1,+.2] in y_1 and [-.15,+.25] in y_2, for all vertex disturbances, we will check the feasibility of the inequalities with "quadprog".
% As quadprog is an "optimization" algorithm  minimising \frac{1}{2}u^THu+f^Tu, we might think on what to minimise; we have several options:
% minimising \|u_{scaled}\|^2 so that we exert the "minimum control effort" to keep the errors within bounds. In this case, H=(E_u^{-1})^2, f=0.
% minimising \|y_{scaled}\|^2, so that we achieve zero output deviation, if feasible; otherwise, we minimise the residual deviation (weighted least squares with a technologically relevant output scaling). In this case:
% \|G_{esc}u+H_{esc}d\|^2=(G_{esc}u+H_{esc}d)^T\cdot (G_{esc}u+H_{esc}d) = u^T\cdot G_{esc}^TG_{esc}\cdot u+2d^TH_{esc}^TG_{esc}\cdot u+d^T\cdot H_{esc}^TH_{esc}\cdot d
%              so, as d is not a decision variable, we would have
%                                 H=G_{esc}^TG_{esc}    , f=2d^TH^T_{esc}G_{esc}.
% Something in between, say, minimising J=\|y_{scaled}\|^2+\rho\cdot \|u_{scaled}\|^2, so weight \rho is used to balance control effort usage versus output residual error magnitude.
% However, as the cost index (in second and third cases) and constraints in quadprog depend on d, in principle, this would require a "feedforward" (measurable d) control structure: this would not be an issue in (known) setpoint tracking, but it is in rejection of unmeasurables disturbances. As we are just interested in "feasibility", not actual "predictive control implementation", we'll just use the first (simplest) option.

lim_elow=[.1;.15]; lim_ehigh=[+.2;+.25]; %we set whatever limits we wish...
for i=1:4
    i
    Aineq=[-Gain;
           Gain];
    Bineq=[lim_elow+dcgain(H)*Vertices_incD(:,i); 
           lim_ehigh-dcgain(H)*Vertices_incD(:,i)];
    quadprog(inv(Eu^2),zeros(1,3),Aineq,Bineq,[],[],Min_incU,Max_incU);
end

% Of course, as perfect cancellation was feasible, so it is imperfect cancellation, this is just to learn how to do it.
% We can zoom out disturbance vertices, and draw some "extended output range" given the error limits.

VertErr=-[lim_ehigh [-lim_elow(1);lim_ehigh(2)]  -lim_elow [lim_ehigh(1);-lim_elow(2)]  ]
ExtendedImageU=[ ImageVerticesU+VertErr(:,1)  ImageVerticesU+VertErr(:,2) ...
         ImageVerticesU+VertErr(:,3) ImageVerticesU+VertErr(:,4)];
keu=convhull(ExtendedImageU(1,:),ExtendedImageU(2,:));
fill(ExtendedImageU(1,keu),ExtendedImageU(2,keu),[1 .4 .5]) %feasible polyhedron + error bounds
hold on
fill(ImageVerticesU(1,k),ImageVerticesU(2,k),'g') %feasible polyhedron
fill(ImageVerticesd(1,kd),ImageVerticesd(2,kd),'c')
fill(VertErr(1,:)*.95+ImageVerticesU(1,2),VertErr(2,:)*.95+ImageVerticesU(2,2),[.75 0.5 1])
hold off, grid on, axis equal
xlabel("y1"),ylabel("y2")
xline(0,'r'),yline(0,'r'),
legend("Extended reachable polyhedron (with error)","Reachable Output Polyhedron 'u'","- Reachable Polyhedron 'd'")

%% D.2) SVD (ellipse) geometry
% SVD/pseudo-inverse solutions are reworking of ordinary least squares... and ordinary least squares have no "constraints" (that would be quadprog). To stay within the underlying assumptions of the methodology, the only thing we may have is "weighted" least squares.
% We'll scale things and weight the "residual error" and the "control effort" jointly, as follows.

WorstCaseLim=[0.1 0.15]; %smallest error bound for each variable
E_residual=diag(WorstCaseLim)

% So, with scaling u=E_u\cdot u_{scaled}, d=E_d\cdot d_{scaled},  \epsilon=E_\epsilon\cdot \epsilon_{scaled}, where \epsilon is the residual output error \epsilon=G\cdot u_{opt}+Hd... when, later on, we do know what u_{opt} should be.
% The scaled version of the above would be:
% \epsilon_{scaled}=G_{scaled}u_{opt,scaled}+H_{scaled}d_{scaled}
% Note that the E_\epsilon matrix is an OUTPUT scaling matrix. For "total" disturbance rejection this scaling was not relevant (results would be identical irrespective of the chosen scaling), but this is no longer the case and the scaling is of course important here, to guide the weighted least squares optimization.
% Our goal will be achieving \|\epsilon_{scaled}\|^2+\|u_{scaled}\|^2\leq 1, because in that case we can guarantee that both \|\epsilon_{scaled}\| and \|u_{scaled}\| are below 1.
% If we denote \nu=\left(\begin{array}{c}\epsilon_{scaled}\\u_{scaled}\end{array}\right), then our goal is achieving \|\nu\|^2\leq1 when \|d_{scaled}\|\leq 1.
%
%
% The equations for \nu are:
% \nu=\left(\begin{array}{c}G_{scaled} \\ I_{3\times 3}\end{array}\right)u_{scaled}+\left(\begin{array}{c}H_{scaled} \\ 0_{3\times 2}\end{array}\right)d_{scaled}=T\cdot u_{scaled}+Q\cdot d_{scaled}
% so the least-squares fit seeking Tu_{scaled}\approx -Qd_{scaled}, i.e., minimising \|\nu\|, will involve the pseudo-inverse T^\dagger of the matrix T multiplying u_{scaled},
% u_{opt,scaled}=-T^\dagger\cdot Qd_{scaled}
% and, as a result, the minimum-norm \nu (optimal solution) would be:
% \nu_{opt}=(-TT^\dagger Q+Q)d_{scaled}=(I-TT^\dagger)Q\cdot d_{scaled}
% so a sufficient condition for our goal to be feasible is that the maximum singular value (maximum gain) of (I-TT^\dagger)Q is less than one.
% Let's do it:

G2esc=inv(E_residual)*Gain*Eu;
H2esc=inv(E_residual)*dcgain(H)*Ed;
T=[G2esc;eye(3)]
Q=[H2esc;zeros(3,2)]
FeedForward2=-pinv(T)*Q %input to FeedForward2 are scaled disturbances, outputs are scaled inputs.
ThingLessThan1=(eye(5)-T*pinv(T))*Q
wcgnu=svd(ThingLessThan1) %Feasible if max gain < 1
1/wcgnu(1) %disturbances could be larger by this factor while keeping feasibility

%
% NOTE: the extension of these ideas to the frequency domain, even with different frequency-dependent maximum residual error and manipulated variable sizes, gives rise to \mathcal H_\infty control.
%
% *Optional discussion: If we separately analyse error and control action size, we may have a slightly finer picture:

wcerr=max( svd(ThingLessThan1(1:2,:)) )
wcu=max( svd(ThingLessThan1(3:5,:)) )
1/wcu %so we have this larger disturbance size margin

% Indeed, if we could minimise "max(wcerr,wcu)" with a different FeedForward2 matrix, that would be a better solution, closer to the "exact" polyhedron manipulations... but such option is, in principle, out of reach for basic SVD/pseudo-inverse techniques.
% In fact, as we want element-by-element less-than-1 results, we could do SVD row by row (which is the same as the Euclidean norm of the row, by the way):

for i=1:5
    svd(ThingLessThan1(i,:))
end

% This would be the largest margin, in which u_{1,opt} would have size 1 for some disturbance in the unit sphere:

1/0.5310

%
%
%
%
%% E) Transient analysis (bandwith), setpoint and disturbance
% We need the "sigma" singular value plot in frequency, with suitable scalings:

Gesc=ss(inv(Ey)*G*Eu); %dynamic system object, not just gain as in earlier sections
Hesc=ss(inv(Ey)*H*Ed); %dynamic system object

% \bullet Setpoint tracking, check frequencies above 0dB:

sigma(Gesc,tf(1)), grid on %el eje vert. tiene unidades logarítmicas de "salidas escaladas"

% We can track arbitrary setpoints up to  2.4 rad/s bandwith (with the maximum amplitudes implied in the scaling, of course).
% Is the manoeuvre at 2.4 rad/s the same as in steady state?

Gbw=evalfr(Gesc, 2.374*1j)
[Ubw,Sbw,Vbw]=svd(Gbw) %yes, it is... Ubw is close to original U.

% \bullet TOTAL Disturbance rejection, check frequencies below 0dB in:

FF=minreal(-Gesc'*inv(Gesc*Gesc')*Hesc); %pseudoinverse is not made by control systems toolbox
size(FF) %two "d" enter, three MV outputs
dcgain(FF) 
sigma(FF, tf(1)), grid on %vertical axis has logarithmic units of "scaled input".

% We have power to cancel the disturbance effect at any frequency below 15 rad/s... in an ideal situation where disturbances were measurable, we had no modelling errors, etc... and we are not limited by measurement noise, delay or whatever: it's an optimistic estimate.
%% Combined setpoint + disturbances (simultaneously, but norm 1)
% If we assume \|r\|^2+\|d\|^2\leq 1, (in scaled units), we can plot the required input amplitude at each frequency, as follows:
%

PinvG=(Gesc'*inv(Gesc*Gesc')); %pseudoinverse is not made by control systems toolbox
Preal2=(Gesc'*inv(Gesc*Gesc')*[-Hesc eye(2)]); %pseudoinverse is not made by control systems toolbox
size(Preal2) %two "d" and two "r" enter, three MV outputs
%sigma(Preal2), grid on
sigma(Preal2, tf(1),'-.',FF,PinvG), grid on %vertical axis has logarithmic units of "scaled input".
ylim([-30 40]), legend("ref+d","0dB","only d","only r")

%