%% SVD decoupling & principal maneuvers: motivation, theory, examples
% © 2024, Antonio Sala Piqueras, Universitat Politècnica de Valencia, Spain. All rights reserved.
%
% Objectives: Understand the "intuitive" meaning of principal maneuvres and SVD decoupling, and its relationship with other "common sense" strategies for multivariable control based on:
%    (a) "sensor subset selection" or "weighted sensor averages",
%    (b) "fusing" actuators by "balanced load sharing".
%
% Presentations in video:
% https://personales.upv.es/asala/YT/V/dsvdintu1EN.html (theory review)
% https://personales.upv.es/asala/YT/V/dsvdintu2EN.html (examples 1 and 3)
% https://personales.upv.es/asala/YT/V/dsvdintu3EN.html (example 2)
% https://personales.upv.es/asala/YT/V/dsvdintu4EN.html (example 2, realizable reference)
% https://personales.upv.es/asala/YT/V/dsvdintu5EN.html (example 4)
% https://personales.upv.es/asala/YT/V/dsvdintu6EN.html   (manual subset/fusion)
%
%
% Table of Contents
% Theory outline
% Example 1: virtual actuator that "combines" two physical actuators (1 x 2 plant)
% Example 2: plant with 3 controlled outputs x 1 manipulated input
% Example 3: plant with 1 controlled output x 3 manipulated inputs
% Example 4: full plant, 3 controlled outputs x 4 manipulated inputs
%%
%%
%% Theory outline
% \bullet Given a "scaled" gain matrix G_{n\times m}, its singular value decomposition  (svd), that is, G_{scaled}=U_{n\times n}S_{n\times m}V_{m\times m}^T allows the identification of the principal maneuvers (output directions: columns of U; input directions: columns of V), and their principal gains.
%
% \bullet  The least squares problems associated with the pseudoinverse of  G_{scaled} also have an SVD interpretation, because
% pinv( G_{scaled} ) = V\underline{S}^{-1}U^T,
% where \underline{S}^{-1}=pinv(S)is calculated by inverting only the non-zero singular values of the diagonal of S(and transposing).
%
% \bullet SVD decoupling:
% From G_{scaled}=USV^T, y=G_{scaled}\cdot u, the change of variable:
%  y^{SVD}=U^Ty , u^{SVD}=V^Tu
% allows writing:
%  y^{SVD}=U^Ty=U^TG_{esc}u=U^T(USV^T)u=Su^{SVD},
% thus, the behavior (in static gain) between the "virtual" output y^{SVD} and the "virtual" input u^{SVD}  is diagonal, even with NON-square plants.
% Since y is the actually measured thing, y^{SVD}=U^Ty must be computed from measurements, so the controllers will calculate:
% u^{SVD}=K(s)\cdot e^{SVD}=K(s)\cdot U^T\cdot (r-y)
% with diagonal K(s)  (hence the name "decoupling").
%  Then the (scaled) physical control action will be u=V\cdot u^{SVD}. So, u^{SVD}=V^Tu isn't actually used, its "inverse" is used instead.
% The SVD-decoupled controller will end up being:
% u=V\cdot K(s)\cdot e^{SVD}=V\cdot K(s)\cdot U^T\cdot (r-y)
%
% If it is decided NOT to control low gain main maneuvers (to avoid saturation/conditioning problems), some elements of the diagonal of K(s) can be forced to zero... or, equivalently, have a smaller-size K(s)  and not multiply by the rows and columns associated with UNcontrolled maneuvers, keeping "q" maneuvers under control:
% u=V_{(:,1:q)}\cdot K_{q\times q}(s)\cdot (U_{(:,1:q)})^T\cdot (r-y)
%
% Let us clarify with some simple examples the intuitive meaning of all these procedures.
%% Example 1: virtual actuator that "combines" two physical actuators (1 x 2 plant)
% Let's imagine a process with a resistance that heats, a fan that cools, each with its units:

G=[1 -10]; 

% Let's do scaling to make things adimensional:

Eu=diag([1 0.2]); %scaling to +1/-1.
Gscaled=G*Eu

% Fan at "full power" (maximum increment till saturation) has double cooling effect than resistance "at full power" has heating.

[U,S,V]=svd(Gscaled)
norm(Gscaled) %singular values of vector equal to Euclidean norm
pinv(Gscaled) %least norm input to achieve unit output increment
V(:,1)/S(1,1) %SVD formula for pseudo-inverse

% Open loop: For each unit of output increment (scaled, of course) we desire, we should increase u_1  "20% of full scale increment"  and decrement u_2 "40% of full scale increment" .
%
% SVD decoupling (closed loop):It would simply amount to having a PID (SISO) that computed u^{SVD}=K(s)\cdot (r-y), because y^{SVD}\equiv y . Then, we would apply it to the actuators with a "load-sharing" strategy:
% u=\left(\begin{array}{c}0.45 \\ -0.89\end{array}\right)\cdot u^{SVD}
%
% The controller K(s) would see a "fictitious" SISO plant with gain S(1,1)=2.236 . Indeed:

Gscaled*V(:,1)

% In principle, roughly speaking, a unit "proportional" gain in K(s) would be what would approximately "saturate" the inputs (formally, u_1^2+u_2^2= 1) in the face of a unit (scaled) reference increase.
%
% *This strategy may be called "load balancing": we distribute the manipulated variable increments proportionally to their effect (after scaling). The SVD or/and the pseudoinverse (trivial, in this case), mathematically formalize the intuitive idea.
% We could choose "split range" (only act with one MV and connect the second one if the first saturates), or other options (cascade) if the dynamics are very different. These issues are out of the SVD-related scope here.
%
%% Example 2: plant with 3 controlled outputs x 1 manipulated input
% *We'll assume everything scaled unless explicitly discussed.
% Assume we have a heating element over a metal piece with three thermocouples whose model is (suitably scaled):

G=[1; 2; 1]
norm(G)
[U,S,V]=svd(G)
pinv(G) %least squares fit, we cannot achieve zero error with 3 setpoints
1/S(1,1)*U(:,1)' %pseudoinverse via SVD

% Interpretation as "projection", SVD decoupling:
% The projection of the combination of outputs given byy^{SVD}_{1\times 1 }=(U(:,1)^T)_{1\times 3 }\, \cdot \, y_{3\times 1 } is actually the only "controllable" output.

U(:,1)'

% It is a "weighted average" in with central sensor counts "double".
%
% The regulator* would compute:
% u=K(s)\cdot e^{SVD}=K(s)\cdot (r^{SVD}-y^{SVD})=K(s)\cdot U(:,1)^T\cdot (r-y)
% *In this case u\equiv u^{SVD}.
%
% The apparent gain that K(s) will see is S(1,1), because U^TG=U^T(US)=S.

U(:,1)'*G

% If user inputs three arbitrary references, the only "realizable" feasible reference will be:r^{feasible}=U(:,1)\cdot y^{SVD}=U(:,1)\cdot U(:,1)^T\cdot r, as, well, control will achieve "zero" error if: y^{SVD}=r^{SVD}=U_{(:,1)}^Tr.
% The components of  r^{SVD} (only one here) are the "length" of the projection of r in the line with direction given by each the chosen columns of U. Multiplying said column of U times such length, we get the actual projection vector in physical units, y=U_{(:,1)}\cdot y^{SVD}=U_{(:,1)}U_{(:,1)}^T\cdot r. Matrix U_{(:,1)}U_{(:,1)}^T is a projection matrix.

T=U(:,1)*U(:,1)'

% *T is the transformation from "physical, user" setpoints to "realizable" ones.
%
%
%
% Example:

ref=[1;1;1]*10
U(:,1)'*ref %setpoint to PID under SVD decoupling
T*ref % realizable setpoint increment

% Let us check the relation with least squares (pseudo-inverse):

u_opt=pinv(G)*ref
y_achieved=G*u_opt

% "Alternate" option to SVD via "common sense", SUBSET:
% With a single actuator, choose only one of the three sensors as the controlled variable (the central one, which is the one on which the actuator has the most effect). It would be basically correct... You would have to decide what is more interesting in the specific application, whether to control only the "central temperature" or if you really want to control the "weighted average" of the temperatures (equivalently, reduce error by least squares), so that if any disturbance affects the sides, the control "reacts" to it. The client could want a different "weighting" of the errors... they would change the scaling or, well, in this case, as it is really 1VM, 1VC and all the numbers are >1, the 3 sensors could really be weighted with great freedom.
%
%
%% Example 3: plant with 1 controlled output x 3 manipulated inputs
% This is like example 1, but thinking of "three" actuators on a controlled variable, instead of two... but the reasoning is identical.

G=[1 2 2]
norm(G)
[U,S,V]=svd(G)
pinv(G)
V(:,1)*1/S(1,1)

% SVD decoupling interpretation: only the actuator maneuver V(:,1) has an effect on the (single) output.
% If we wish to increment output in r units, we should act with pinv(G)\cdot r input incremental units; any other input increment achieving  r will score a larger u_1^2+u_2^2+u_3^2.
% Or, well, with SVD decoupling,we will set a PID to compute u^{SVD}=K(s)\cdot (r-y), because y^{SVD}\equiv y. Then, we would apply it to actual manipulated variables in physical units as:
% u=\left(\begin{array}{c}0.33 \\ 0.67 \\ 0.67\end{array}\right)\cdot u^{SVD}
%
% If I change variable to u=V(:,1)\cdot u^{SVD} then the apparent gain from u^{SVD} to y will be S(1,1); indeedy=USV^Tu=(US)V^T V(:,1)\cdot u^{SVD}=S_{11}\cdot u^{SVD}, as U=1. Controller K(s) would see a "fictitious" SISO process with gain S(1,1)=3 .

G*V(:,1)

%
%% Example 4: full plant, 3 controlled outputs x 4 manipulated inputs

G=[1 -4.5 3 -12; 2 -8 2 -8; 3 -12 1 -4.5]
rank(G)

% Let us scale to [-1,+1]:

Ey=diag([4.5 5 5]);
Eu=diag([1.5 0.6 1.5 0.6]);
Gscaled=inv(Ey)*G*Eu
[U,S,V]=svd(Gscaled)

% Obviously, apart from a very small minimum gain, the condition number is unacceptable:

cond(Gscaled)

% If we only wish to control TWO things, that can be done well, according to the condition-number criterion:

S(1,1)/S(2,2)

% We may carry out SVD decoupling by controlling two principal maneuvres:
% y^{SVD}=U(:,1:2)^T\cdot y, and u^{SVD}=V(:,1:2)^Tu; actually, the second transform will not be used, because we decide u^{SVD} to transform tou=V(:,1:2)\cdot u^{SVD}.

U(:,[1 2])' %from physical sensors to virtual SVD ones
V(:,[1 2]) %from virtual "uSVD" to physical "u".

%
% Realizable reference:

T=U(:,[1 2])*U(:,[1 2])'
r=[1; .5; 0.];
T*r %realizable

%
% Alternative to SVD decoupling (sensor subset): Controlling only left and right sides?.
% If we disregard the central "sensor" (at least, as a "controlled variable"; indeed, we may use it on a "state observer" even if it is not a controlled variable, if we have such a measure), we have:

subspace(U(:,1:2),[1 0;0 0;0 1])*180/pi %good, angle between coordinate planes and principal maneuvres
subspace(U(:,1:2),[0 0;1 0;0 1])*180/pi %not good
subspace(U(:,1:2),[1 0;0 1;0 0])*180/pi %not good
G2=Gscaled([1 3],:)
[U2,S2,V2]=svd(G2)
cond(G2) %acceptable

% If I chose a different sensor selection, I would get a lower "minimum gain":

svd(Gscaled([1 2],:))
svd(Gscaled([2 3],:))

% which means that I would see "less increments" in my sensors in the worst-case maneuver... the motion I observe with three sensors will be significantly less "observable" with sensors [1 2] or [2 3] than with [1 3].
%
% But we would need four manipulated inputs and SVD decoupling on it, so, well, we could have done it on the original plant G_{3\times 4}... maybe selecting only 2 actuators would do?

svd(G2(:,[1 2]))
svd(G2(:,[1 3]))
svd(G2(:,[1 4]))
svd(G2(:,[2 3]))
svd(G2(:,[2 4])) %**** GOOD... almost unit mingain, but not enough
svd(G2(:,[3 4]))
G2x2=G2(:,[2 4])
cond(G2x2)
RGA=G2x2.*inv(G2x2') %good RGA...

% Another alternate option to SVD: Common-sense "actuator fusion"?

Gscaled
V2(:,[1 2])
V2(2,[1 2])./V2(1,[1 2])
V2(4,[1 2])./V2(3,[1 2])
F=[1 -1.58 0 0;0 0 1 -1.56]'
F=F*diag([1/norm([1 -1.58]),1/norm([1 -1.56])]) %por no cambiar "escalado"
G3=G2*F 

% G3 is quite similar to the previous G2x2 because one actuator "dominates" a lot in the weighting... If it is a prototype, a plant could be designed with only 2 actuators... But if I already have the 4 of them, we move on.

svd(G3)
cond(G3)
RGA=G3.*inv(G3')

% RGA is reasonable, we could try PIDs with those  "fused" actuators two by two... or the "fusion of 4 actuators" given by the SVD of G2, of course... OR/AND the fusion of 3 sensors given by the SVD by original Gesc, obviously.
%