%% Demonstration of multipoly and sosopt packages
% Demonstrated during Lecture 4 of CDS 270 on Oct 13th.
%
% By U. Topcu & A. Lamperski

%% Basic manipulations with multipoly
% Define two polynomial objects x1 and x2
pvar x1 x2
%%
% Form the polynomial p = 2*x1^4 + 2*x1^3*x2 - x1^2*x2^2 + 5*x2^4
p = 2*x1^4 + 2*x1^3*x2 - x1^2*x2^2 + 5*x2^4
%%
% Check the fields of p
p.coef
%%
p.deg
%% 
p.var

%% Vector representation example
x = [x1;x2];
%%
% Form the monomials vector
w = monomials(x,0:4);
%%
% Compute the coefficients vector
c = poly2basis(p,w);
%%
% Does the decomposition work?
p - c'*w

%% Gram matrix representation
% Compute the monomials vector z, a particular Q for the equality p =
% z'*Q*z, and all homogenous solutions.
[z,Q,N] = gramsol(p);
Q = full(reshape(Q,[3 3]));
N = full(reshape(N,[3 3]));
%%
% Does the decomposition work?
p-z'*Q*z
%%
% Check the homogenous solution.
z'*N*z

%% Is p an SOS polynomial?
% p is SOS iff there exists lambda such that Q+lambda*N is psd.
%%
% Let's take a range for lambda and check the minimum eigenvalues of
% Q+lambda*N for values of lambda on a grid in this range.
lambda = linspace(-2,10,100);
for i1 = 1:length(lambda)
    mineig(i1) = min(eig(Q+lambda(i1)*N));
end
%%
% Plot the result
plot(lambda,mineig,'k','linewidth',2);
xlabel('\lambda','fontsize',18); ylabel('min(eig((Q+lambda*N))','fontsize',18);

%% SOS feasibility using sosopt
% Check if p is a SOS polynomial using sosopt
[feas,z,Q] = issos(p);
%%
% Check if the decomposition works
p - z'*Q*z
%%
% Is the Gram matrix psd?
min(eig(Q))

%% Montzkin polynomial
% What happens if we call issos for a Montzkin polynomial?
pMon = x1^2*x2^4 + x1^4*x2^2 + 1 - 3*x1^2*x2^2;
[feas,z,Q] = issos(pMon);
%%
% Check the feasibility
feas




%% SOS synthesis example
% Given polynomials f0 and f1, find the smallest value of alpha such that f0+alpha*f1
% is SOS

%% 
% Problem set-up with polynomial toolbox and sosopt
pvar x1 x2 alpha;
f0 = -x1^4 + 2*x1^3*x2 + 9*x1^2*x2^2 - 2*x2^4;
f1 = x1^4 + x2^4;
x = [x1;x2];
obj = alpha;
[info,dopt,sossol]=sosopt(f0+alpha*f1,x,obj);
%%
% s is f0+alpha*f1 evaluated at the minimal alpha
s = sossol{1};
%% 
% z and Q are the Gram matrix decomposition of s
z=sossol{2}; Q=sossol{3};
%% 
% Feasibility of sosopt result
info.feas
%%
% Minimal value of alpha
dopt
%%
% Verify s is f0+alpha*f1 evaluated at alpha = 2.00
s-subs( f0+alpha*f1, dopt)
%%
% Verify z and Q are the Gram matrix decomposition of s
s-z'*Q*z
%% 
% Verify Q is positive semi-definite
min(eig(Q))



%% Global lower bound for a polynomial
% Given a polynomial p,
%% 
% Problem of interest:     min_x  p(x) 
%%
% Write the problem as 
%%
% max_{alpha,x} alpha subject to p(x) - alpha >= 0 for all x
%% 
% This is an hard problem, but if we replace psd constraint by a SOS
% constraint, we can compute a lower bound on the optimal value of alpha
% using sosopt.
%% 
% The following problem can be solved using sosopt:
%% 
% max_{alpha,x} alpha subject to p(x) - alpha >= 0 is SOS in x

pvar x1 x2 x3 alpha;
x = [x1;x2;x3];
p = x1^4 + 4*x3^4 - 2*x1^2*x3 + 4*x1*x3^2 - 4*x2*x3^2 - 5.16*x1^2 - 2*x1*x2 + x2^2 + 6.24*x3^2 + 2.62*x1 - 2.62*x2 + 6.16*x3 + 4.3325;
obj = -alpha;
[info,dopt,sossol]=sosopt(p-alpha,x,obj);
%% 
% Feasibility of sosopt result
info.feas
%%
% Optimal value of alpha
dopt
%%
% Then the conclusion is that p is (globally) greater than or equal to the optimal
% value of alpha



%% Global Stability Analysis
% Later in the course, we will learn why the following statement is correct
% and in fact many more extensions of such statement.

%%
% Let a dynamical system be governed by dx/dt = f(x) with f(0) = 0.
%%
% Then, if there exists a positive definite function V such that V(0) = 0
% and dV/dt < 0 for all x, then the origin is a globally, asymptotically stable equilibrium point
% for the system. 
%% 
% Now, we use sosopt to find a quadratic V that satisfies the above
% conditions for a specific vector field f.

%%
% Define the vector field
pvar x1 x2;
x = [x1;x2];
x1dot = -x1 - 2*x2^2;
x2dot = -x2 - x1*x2 - 2*x2^3;
f = [x1dot; x2dot];

%%
% Monomials vector to form a V using polydecvar's vector option
zV = monomials(x,2);
%%
% Candidate V is of the for V(X) = c'*zV(x)
V = polydecvar('c',zV,'vec');
%%
% Constraint 1 : V(x) - L1 \in SOS - this is to make sure that V is
% positive definite
L1 = 1e-6 * ( x1^2 + x2^2 );
sosconstr{1} = V - L1;
%%
% Constraint 2: -Vdot - L2 \in SOS
L2 = 1e-6 * ( x1^2 + x2^2 );
Vdot = jacobian(V,x)*f;
sosconstr{2} = -Vdot - L2;
%%
% Solve with feasibility problem
[info,dopt,sossol] = sosopt(sosconstr,x);
%% 
% Is the problem feasible?
info.feas
%% 
% What does V look like
Vsol = subs(V,dopt)