%% Maximum volume ellipsoid in intersection of ellipses and polyhedra: Matlab+Yalmip+SeDuMi example
% © 2024, Antonio Sala. Universitat Politecnica de Valencia, Spain. All rights reserved.
% Presentation in video: https://personales.upv.es/asala/YT/V/elinncEN.html
% Objective: illustrating LMIs (semidefinite programming) in geometric problem with ellipses not centered at the origin.
% *This code worked with Matlab R2024a+yalmip:R20230609+sedumi:1.3
%% The region in which we'll fit the ellipse

if ~exist('yalmiptest','file') %add toolboxes to path
    addpath("~/bin/MATLAB/tbxmanager/")
    tbxmanager restorepath
end %depends on your installation method/source

% An ellipsoid (x-c_1)^TQ_1^{-1}(x-c)\leq 1 with:

Q1=[3 -1;-1 4]*2;
c1=[2;1];

% A polyhedron Ax\leq b with:

A=[1 -.1; -1 0; 0.1 1; 0.2 -1];
b=[3; 0.1; 3; 2];
Nfaces=size(A,1)

% Let us plot the intersection region:

Ellipfun=@(x1,x2) 1-([x1;x2]-c1)'*inv(Q1)*([x1;x2]-c1); %>=0, we are in the ellipsoid
for i=1:Nfaces
    PolyFun{i}=@(x1,x2) -A(i,:)*[x1;x2]+b(i); %>=0 we are at the right side of each face
end
SetFun=@(x1,x2) min([Ellipfun(x1,x2),PolyFun{1}(x1,x2),PolyFun{2}(x1,x2),PolyFun{3}(x1,x2),PolyFun{4}(x1,x2)]);
%If the minimum is >=0, all of them are >=0. This is the set boundary.
axx=[-2 5 -5 5];
fimplicit(Ellipfun,axx)
hold on
for i=1:Nfaces
    fimplicit(PolyFun{i},axx)
end
grid on
fimplicit(SetFun,axx,LineWidth=3)
hold off, axis equal

%% Finding the maximum-area ellipse in the region
% For convenience, let us look for a maximum volume ellipsoid in the form \|H^{-1}(x-c)\|\leq 1, which means (x-c)^TH^{-2}(x-c)\leq 1... we are assumming symmetric H with no loss of generality H^TH=Q has a symmetric square root (we can get it from eigenvectors and square root of eigenvalues).

sdpvar cx cy lam
 c=[cx;cy];

H=sdpvar(2); %symmetric square root of sought ellipsoid
%sdpvar radius  %largest circle
%H=radius*eye(2);
LMIs= H>=0 ; %we with positive square root of Q=H^TH.

% First non-trivial LMI: target ellipsoid inside the above-defined one, see
% https://web.stanford.edu/~boyd/lmibook/lmibook.pdf page 46, (3.17)

LMIs=[LMIs;
    [-Q1      c1-c   H; ...
     (c1-c)' lam-1  0 0;...
      H'    [0;0] -lam*eye(2)]<=0 ;
      lam >=0]; %last one is actually redundant, but, well, it's in Boyd's book

% Second group of LMIs: target ellipsoid inside polyhedron, face by face, see
% https://users.ece.utexas.edu/~cmcaram/EE381V_2012F/Lecture_17_Scribe_Notes.final.pdf page 17-6, equation (17.11):

for i=1:Nfaces
    LMIs=[LMIs;
        [b(i)-A(i,:)*c       A(i,:)*H; ...
           H'*A(i,:)' (b(i)-A(i,:)*c)*eye(2)]>=0 ];
end
sol=optimize(LMIs,-geomean(H)) %geometric mean is propotional to determinant, maximization
Hval=value(H)
cval=value(c)
fimplicit(SetFun,axx*.8,LineWidth=2)
hold on
EllipfunSOL=@(x1,x2) 1-([x1;x2]-cval)'*inv(Hval^2)*([x1;x2]-cval);
fimplicit(EllipfunSOL,axx*.8,LineWidth=2)
plot(cval(1),cval(2),'x',LineWidth=3)
hold off, grid on, axis equal
legend("Set boundary", "Extremal Ellipse","center",Location="best")

%
%
%
%