%% Stability analysis of a PWA system by searching for a piecewise quadratic Lyapunov function
% Example 2 from Johansson, M., and A. Rantzer. Computation of Piecewise Quadratic Lyapunov Functions for Hybrid Systems. IEEE Transactions on Automatic Control 43, no. 4 (April 1998): 555–59.  https://doi.org/10.1109/9.664157.
% The systems switches between three piecewise affine systems \dot x = A_i x + b_i, \; i=1,2,3:

A1 = [-10 -10.5; 10.5 9]; b1 = [-11; 7.5];
A2 = [-1 -2.5; 1 -1]; b2 = [0; 0];
A3 = [-10 -10.5; 10.5 -20]; b3 = [11; 50.5];

% depending on the value of x_1: x_1 < -1, -1 \leq x_1 \leq 1, x_1 > 1.
% Hybrid equations for this system in the form of a class definition are in the PiecewiseAffineSystem.m file, make sure you have the file in the same directory.
%
%% Solving for the state response and plotting it
% Now we compute and plot some state trajectory for the system:

sbs = PiecewiseAffineSystem();

% Initial conditions
x0 = [-3; -3];
q0 = 1;

% Time spans
tspan = [0, 5];
jspan = [0, 20];

% Specify solver options.
config = HybridSolverConfig('AbsTol', 1e-7, 'RelTol', 1e-7,'Refine',16);

% Compute solution
sol = sbs.solve([x0;q0], tspan, jspan, config);

% Plotting the trajectory

figure(1)
plotFlows(sol);

% and plotting the state portrait

figure(2)
hp = HybridPlotBuilder().plotPhase(sol.select([1,2]));
grid on
axis auto
hold on
x1l = -ones(1,100);
x1u = ones(1,100);
x2l = linspace(-5,4,100);
x2u = linspace(-5,4,100);
plot(x1l,x2l,'-.')
plot(x1u,x2u,'-.')
hold off

%
%% Partitioning the state space into three regions (corresponding to the three modes)

% q = 1: x1 <= -1
% 
% q = 2: -1 <= x1 <= 1
% 
% q = 3: x1 >= 1

Q1 = zeros(3,3);
Q1(1:2,3) = [-1; 0];
Q1(3,1:2) = [-1; 0];
Q1(3,3) = -2;

Q2l = zeros(3,3);
Q2l(1:2,3) = [1; 0];
Q2l(3,1:2) = [1; 0];
Q2l(3,3) = 2;

Q2u = zeros(3,3);
Q2u(1:2,3) = [-1; 0];
Q2u(3,1:2) = [-1, 0];
Q2u(3,3) = 2;

Q3 = zeros(3,3);
Q3(1:2,3) = [1; 0];
Q3(3,1:2) = [1, 0];
Q3(3,3) = -2;

Ibar = eye(3,3);
Ibar(3,3) = 0;

A1bar = zeros(3,3);
A1bar(1:2,1:2) = A1;
A1bar(1:2,3) = b1;
A1 = A1bar;

A2bar = zeros(3,3);
A2bar(1:2,1:2) = A2;
A2 = A2bar;

A3bar = zeros(3,3);
A3bar(1:2,1:2) = A3;
A3bar(1:2,3) = b3;
A3 = A3bar;

%
%% Building the LMI(s)
% First, setting the variables

P1 = sdpvar(3,3); P2 = sdpvar(3,3); P3 = sdpvar(3,3);       % These are actually P1bar, P2bar and P3bar, but we shorten for brevity.

alpha = 1e-5; 
beta = sdpvar(1,1);
gamma = 1e-5; 

u1 = sdpvar(1,1); u3 = sdpvar(1,1);
u2l = sdpvar(1,1); u2u = sdpvar(1,1);

v1 = sdpvar(1,1); v3 = sdpvar(1,1);
v2l = sdpvar(1,1); v2u = sdpvar(1,1);

z1 = sdpvar(1,1); z3 = sdpvar(1,1);
z2l = sdpvar(1,1); z2u = sdpvar(1,1);

% Then, we need to define

F12 = [0; 1]; l12 = [-1; 0];
F23 = [0; 1]; l23 = [1; 0]; 

% that will be used to define the boundaries between the regions of the state space corresponding to individual discrete states (modes). Namely, boundary between the region 1 and 2 is given by x = F_{12}z + l_{12}, where z (a scalar in this case) parameterizes the point on the boundary (a line in this case).
% Finally, let's define the LMIs

LMI = [u2l>= 0, u2u >= 0, u1 >= 0, u3 >= 0, ...
    v2l >= 0, v2u >= 0, ...
    z2l >= 0, z2u >= 0, z1 >= 0, z3 >= 0, ...
    beta >= alpha;
    P2-u2l*Q2l-u2u*Q2u >= alpha*Ibar, ...
    P2+v2l*Q2l+v2u*Q2u <= beta*Ibar, ...
    P1-u1*Q1 >= alpha*Ibar, ...
    P1+v1*Q1 <= beta*Ibar, ...
    P3-u3*Q3 >= alpha*Ibar, ...
    P3+v3*Q3 <= beta*Ibar, ...
    A2'*P2+P2*A2+z2l*Q2l+z2l*Q2l <= -gamma*Ibar, ...
    A1'*P1+P1*A1+z1*Q1 <= -gamma*Ibar, ...
    A3'*P3+P3*A3+z3*Q3 <= -gamma*Ibar, ...
    F12'*(P1(1:2,1:2)-P2(1:2,1:2))*F12 == 0, ...
    F23'*(P2(1:2,1:2)-P3(1:2,1:2))*F23 == 0, ...
    F12'*(P1(1:2,1:2)-P2(1:2,1:2))*l12 + F12'*(P1(1:2,3)-P2(1:2,3)) == 0, ...
    F23'*(P2(1:2,1:2)-P3(1:2,1:2))*l23 + F12'*(P2(1:2,3)-P3(1:2,3)) == 0, ...
    l12'*(P1(1:2,1:2)-P2(1:2,1:2))*l12 + 2*(P1(1:2,3)-P2(1:2,3))'*l12 + (P1(3,3)-P2(3,3)) == 0, ...
    l23'*(P2(1:2,1:2)-P3(1:2,1:2))*l23 + 2*(P2(1:2,3)-P3(1:2,3))'*l23 + (P2(3,3)-P3(3,3)) == 0];

% Now we can solve the LMI feasibility problem (there is no cost function in the optimization, we just check if the problem has any solution)

options = sdpsettings('verbose',0,'solver','mosek');

lmisol = optimize(LMI,[],options);
lmisol.info

% Let's relabel the matrices back to their "barred" names. Note that P2 corresponds to the homogeneous form – P(1:2,3) and P(3,3) are zero –, hence we name the 2x2 upper left submatrix as P2 for convenience.

P1bar = value(P1) 
P2bar = value(P2), P2 = P2bar(1:2,1:2);
P3bar = value(P3)

%
%% Plotting the piecewise quadratic Lyapunov function
% Note that the piecewise definition of the function is given at the end of the script (this is how Matlab requires it...)

x1 = -3.0:.01:2.0;
x2 = -5.0:.01:4.0;
[X1,X2] = meshgrid(x1,x2);
V = zeros(size(X1));
for i = 1:length(x2)
    for j = 1:length(x1)
        x = [x1(j);x2(i)];
        V(i,j) = mlf(x,P1bar,P2bar,P3bar);
    end
end

figure(3)
mesh(X1,X2,V)
xlabel('x_1')
ylabel('x_2')
zlabel('V(x)')
title('Piecewise quadratic Lyapunov function')

% Rotating with the above 3D plot of the piecewise quadratic Lyapunov function is highly recommended to get some appreciation for it.
% And we also plot the contours for the piecewise quadratic Lyapunov function. Note how in each vertical segment (discrete state) the solution (the thick curve) always enters a lower-value sublevel set but never the other way round.

figure(4)
contour(X1,X2,V,30)
hold on
plot(sol.x(:,1),sol.x(:,2),'LineWidth',3)
plot(x1l,x2l,'-.')
plot(x1u,x2u,'-.')
hold off
xlabel('x_1')
ylabel('x_2')
title('Contours of piecewise quadratic Lyapunov function')

% We can also plot how this particular piecewise defined Lyapunov function evolves as the solution evolves.

figure(5)
hp5 = HybridPlotBuilder().plotFlows(sol,@(x) mlf(x,P1bar,P2bar,P3bar));
xlabel('t')
ylabel('V(x(t))')
title('Evolution of piecewise quadratic Lyapunov function')
grid on

% Above we see that indeed, while staying in a given discrete state, the function is smooth and decreases monotonically. At the moments of switching from one discrete state to another, Lyapunov function is continuous, albeit nonsmooth.
%
%% Definition of the piecewise Lyapunov function for this example

function V = mlf(x,P1bar,P2bar,P3bar)
    x = x(1:2);
    if abs(x(1)) <= 1
        V = x'*P2bar(1:2,1:2)*x + 2*P2bar(3,1:2)*x + P2bar(3,3);
    elseif x(1) < -1
        V = x'*P1bar(1:2,1:2)*x + 2*P1bar(3,1:2)*x + P1bar(3,3);
    elseif x(1) > 1
        V = x'*P3bar(1:2,1:2)*x + 2*P3bar(3,1:2)*x + P3bar(3,3);
    end
end