%% LMIs and Schur Formula, example: minimum distance between ellipses, generalization to LMI sets (intersection of ellipsoids, for simplicity)
% © 2024, Antonio Sala Piqueras, Universitat Politècnica de València. All rights reserved.
% Objective: calculate the minimum distance (and the points that achieve it) between two general LMI sets (well, the simplest example is intersection of ellipsoids).
% Presentation in video:
% https://personales.upv.es/asala/YT/V/distlmisetEN.html
%
% *Let's load the necessary toolboxes (Yalmip, Sedumi in this case).

if ~exist("yalmiptest")
    addpath("~/bin/MATLAB/tbxmanager/")
    tbxmanager restorepath
end

%% Problem statement
% Let us define ellipses as (x-c)^T\cdot P\cdot (x-c)\leq 1; or with the equivalent inverse form (equiv. if non degenerate): (x-c)^T\cdot Q^{-1}\cdot (x-c)\leq 1. Both P and Q are assumed to be positive definite matrices.

c{1}=[2;0];
Q{1}=[0.5 0;0 1];

c{2}=[0.25;1.7];
Q{2}=[5 -2;-2 1.5];

% Shape matrix Q is "proportional" to the square "size" of the ellipsoid, P es "inverse proportional".

for k=1:2
    P{k}=inv(Q{k});
    fimplicit(@(x,y) ([x;y]-c{k})'*P{k}*([x;y]-c{k})-1,LineWidth=2), hold on
end
hold off, grid on, axis equal

% Schur's formula: A-B^TQ^{-1}B\succ 0 and
%  Q\succ 0    iff  \left(\begin{array}{cc}A &B^T\\ B &Q\end{array}\right)\succ 0.
% Identifying B \equiv (x-c), A=1, we have that  1-(x-c)^TQ^{-1}(x-c)> 0 and Q\succ 0  if and only if:
% \left(\begin{array}{cc}1 &x^T-c^T\\ x-c &Q\end{array}\right)\succ 0
%
% Let's introduce a search point on each ellipse as decision variable:
% \left(\begin{array}{cc}1 &x_1^T-c_1^T\\ x_1-c_1 &Q_1\end{array}\right)\succ 0          \left(\begin{array}{cc}1 &x_2^T-c_2^T\\ x_2-c_2 &Q_2\end{array}\right)\succ 0

x{1}=sdpvar(2,1);
x{2}=sdpvar(2,1);
LMIs=[];
for k=1:2
    LMIs=[LMIs; ...
        [1 x{k}'-c{k}'; x{k}-c{k}  Q{k}]>=0 ];
end

%
%
% Distance between two points being smaller than d amounts to sayingd^2>\|x_1-x_2\|^2=(x_1-x_2) ^T(x_1-x_2), i.e., d^2-(x_1-x_2) ^T(x_1-x_2)> 0, thus Schur formula says:
% \left(\begin{array}{cc}d^2 &x_1^T-x_2^T\\ x_1-x_2 &I_{2\times 2}\end{array}\right)\succ 0
% We will add that to the LMIs, changing d^2 nonlinear to "dsquared= d^2" linear in dsquared.

dsquared=sdpvar();
LMIs=[LMIs; ...
    [dsquared x{1}'-x{2}'; x{1}-x{2} eye(2)]>=0 ];
           

%
% If we look for the minimum distance, we have to find the minimum dsquared for which there are feasible points x{1} and x{2}.

sol=optimize(LMIs,dsquared)
dsquared
sqrt(value(dsquared))
solx{1}=value(x{1});
solx{2}=value(x{2});
for k=1:2   
    fimplicit(@(x,y) ([x;y]-c{k})'*P{k}*([x;y]-c{k})-1,LineWidth=2), hold on
end
for k=1:2
    plot(solx{k}(1),solx{k}(2),'x',LineWidth=3)
end
hold off, grid on, axis equal

%% Generalization: minimum distance between "LMI sets"
% If the sets are representable with LMI constraints, we can calculate their minimum distance... Intersections of ellipses, direct sums, convex hulls, and other convex sets with polynomial boundaries [ https://homepages.laas.fr/henrion/courses/edsys04/edsys04_3.pdf   Didier Henrion, LAAS, Tolouse]
% Well, some of these sets are "lifted LMI sets", lower-dimensional projections of "true" LMI sets. Details out of the scope of this introductory material.
%
% Let's introduce a search point on each ellipse as decision variable and let's add that the set "1" is the INTERSECTION of the previous ellipse and another one:
% \left(\begin{array}{cc}1 &x_1^T-c_1^T\\ x_1-c_1 &Q_1\end{array}\right)\succ 0          \left(\begin{array}{cc}1 &x_2^T-c_2^T\\ x_2-c_2 &Q_2\end{array}\right)\succ 0
%
% \left(\begin{array}{cc}1 &x_1^T-c_3^T\\ x_1-c_3 &Q_3\end{array}\right)\succ 0
%
% So very minor changes to the code solve the problem: just add the last LMI to the constraints and repeat optimization.

c{3}=[3;-.2];
Q{3}=[1 0.2;0.2 0.5]; P{3}=inv(Q{3});
Set1Func=@(xi) max((xi-c{1})'*P{1}*(xi-c{1}), (xi-c{3})'*P{3}*(xi-c{3}))-1
fimplicit(@(x,y) ([x;y]-c{1})'*P{1}*([x;y]-c{1})-1,':'), hold on
fimplicit(@(x,y) Set1Func([x;y])), grid on, axis equal
fimplicit(@(x,y) ([x;y]-c{3})'*P{3}*([x;y]-c{3})-1,':'), hold off

% We add the extra LMI here:

LMIs=[LMIs; [1 x{1}'-c{3}'; x{1}-c{3}  Q{3}]>=0 ];

% And, redoing optimization, we get:

sol3=optimize(LMIs,dsquared)
value(dsquared)
sqrt(value(dsquared))
solx{1}=value(x{1});
solx{2}=value(x{2});

fimplicit(@(x,y) ([x;y]-c{1})'*P{1}*([x;y]-c{1})-1,':'), hold on
fimplicit(@(x,y) Set1Func([x;y]),LineWidth=2)
fimplicit(@(x,y) ([x;y]-c{2})'*P{2}*([x;y]-c{2})-1,LineWidth=2), hold on

for k=1:2
    plot(solx{k}(1),solx{k}(2),'x',LineWidth=3)
end
hold off, grid on, axis equal

%
%