%% Introduction to sums of squares using SOSTOOLS for Matlab
%% Testing nonnegativeness of a polynomial
% Define variables

clear
pvar x1 x2

% Using the variables build a polynomial

p = 2*x1^4 + 2*x1^3*x2 - x1^2*x2^2 + 5*x2^4

% The properties of the polynomial can be queried

p.varname
p.degmat
p.coefficient
vars = [x1; x2]
prog = sosprogram(vars)
prog = sosineq(prog,p);

% And call the solver (choose from 'sedumi', 'sdpt3', 'csdp', 'mosek', ..., whatever is set up on your computer)

solver_opt.solver = 'mosek'
[prog,info] = sossolve(prog,solver_opt)

% In fact, even before we started solving the corresponding SDP problem, we could have asked for the SOS representation of the polynomial

[Q,Z,f] = findsos(p)

%
%% Testing Lyapunov stability

clear

% State variables

pvar x1 x2 x3 x4
vars = [x1; x2; x3; x4]

% Definition of the system by giving the right hand side vector function f

f = [-x1 + x2^3 - 3*x3*x4;
    -x1 - x2^3;
    x1*x4 - x3;
    x1*x3 - x4^3]

% First, initialize the sum of squares program

prog = sosprogram(vars)

% Define the polynomial Lyapunov function V(x)

[prog,V] = sospolyvar(prog,[x1^2; x2^2; x3^2],'wscoeff')

% Next, define SOSP constraints. First, we want to enforce the constraint that
% V(x) > 0.
% Unfortunately, strict inequalities cannot be enforce directly. Instead, we formulate the auxiliary nonstrict inequality
% V(x) \geq \gamma \|x\|^2,
% where some small \gamma
%  is chosen. Therefore, the constraint is encoded as

gamma = 1e-3;
prog = sosineq(prog,V-gamma*(x1^2+x2^2+x3^2))

% Then we have the other inequality, which reads that
% \nabla V(x)^\top f(x) \leq 0.
% in which the inequality is nonstrict, indeed, because we are only testing for Lyapunov stability (and not asymptotic stability). This can be encoded as

expr = -(diff(V,x1)*f(1)+diff(V,x2)*f(2)+diff(V,x3)*f(3))
prog = sosineq(prog,expr)

% Now we call the solver (choose from 'sedumi', 'sdpt3', 'csdp', 'mosek', ...)

solver_opt.solver = 'sedumi'
prog = sossolve(prog,solver_opt)

% Finally, extract the solution

SOLV = sosgetsol(prog,V)

% Obviously no Lyapunov function has been found of the prescribed (quadratic) degree. We can give it a try with higher-degree polynomials.

clear prog
prog = sosprogram(vars);
z = monomials([x1; x2; x3; x4],2:4);
[prog,V] = sospolyvar(prog,z,'wscoeff');
prog = sosineq(prog,V-gamma*(x1^2+x2^2+x3^2));
expr = -(diff(V,x1)*f(1)+diff(V,x2)*f(2)+diff(V,x3)*f(3));
prog = sosineq(prog,expr);
solver_opt.solver = 'sedumi';
prog = sossolve(prog,solver_opt);
SOLV = sosgetsol(prog,V)

% Alternatively, we can use the higher-level function findlyap for find a Lyapunov function of a prescribed degree.

solV2 = findlyap(f,vars,6)