%%%%%  M file corresponding to the numerical example of the TAC paper about uniting CLF
%%%%%
%%%%%  DATE : 5th of may 2009

clear all;
close all;

% The system is 
% dot x1 = -x1 + x3
% dot x2 = x1^2 -x2 -2x1x3 + x3
% dot x3 = -x2 + u

%Initial value of the model
x10 = 1;
x20 = 1;
x30 = 1;


% The first order approximation of the system is 
% dot x = F x + G u with
F = [[-1 0  1];[0 -1 1];[0 -1 0]];
G = [0;0;1];

% For this system a CLF can be designed as
% Vi = x1^2 + (x1^2 + x2)^2 + x3^2
% Note that the quadratic approximation of this CLF is x^TPix with
Pi = eye(3);

% We design a LQ controller on this one
% Parameter of the LQ controller
q = rand(3,3);
Q =  [[0.8    0.6    0.3];
    [0.6    0.6    0.5];
    [0.3    0.5    1]];%q'*q;
R = 1;

% We solve the riccatti equation
[P0, L, K0, rr] = care(F, G, Q, R, zeros(3,1), eye(3));

% The local controler phi0 is given as u = -G0'*P0/R

% The problem is now to unite phii and phi0,

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% LMI Condition %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Following the theory, we need to check if a lmi is satisfied
% settings of the solver
opt = sdpsettings('solver','sedumi','verbose',0);
K = sdpvar(1,3);

% Matrix that has to be negative for the local design
Mat0 = [P0*(F+G*K) + (F+G*K)'*P0];
Mati = [Pi*(F+G*K) + (F+G*K)'*Pi];

LMI =  [Mat0<0, Mati<=0];
solvesdp(LMI, [], opt);

% we check if the LMI was solved
if max([max(eig(double(Mat0))), max(eig(double(Mati)))])>0 
    % In that case the LMI couldn't be solved
    disp('Problem with the Uniting');
    quit;
else
    disp('LMI solved, the controller can be united')
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Controller synthesis %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Selection of the parameters which separate the state space for the
% uniting CLF problem.
% We introduce a new parameter epsilon which will enable us to be closer
% from the origin.
eps = 0.5;
% We want ri x'P0x < R0 x'Pix, 
ri = eps * 0.7 * min(eig(Pi));
R0 = eps * 1.3 * max(eig(P0));
% and ri x'P0x \leq r0 x'Pix, 
r0 = ri * max(eig(P0)) / min(eig(Pi));
% and Ri x'P0x \leq R0 x'Pix, 
Ri = R0 * min(eig(Pi)) / max(eig(P0));

% Selection of the high-gain parameter of the controller
k = 1;

% Number of iteration
NbIter = 3000;

% integration step size
dt = 0.001;

% Initialization of the state vector for the united contoller
xv = zeros(3,NbIter);
xv(:,1) = [x10; x20; x30];

% Initialization of the state vector for the global controller (without
% modification)
xvu = zeros(3,NbIter);
xvu(:,1) = [x10; x20; x30];

% Initialization of the vector storing the values of the Lyapunov function
Vu = zeros(NbIter,1);
V = zeros(NbIter,1);

% Initialisation of the vector storing the values of the cost
% J = int_0^t R u^2 + x^TQx dt
J = zeros(NbIter,1);
%dJdt = zeros(NbIter,1);
Ju = zeros(NbIter,1);
%dJdtu = zeros(NbIter,1);

Ju(1) = 0;
J(1) = 0;

% Initialisation of the vector storing the values of the control
Vect_u = zeros(NbIter,1);


% We remind that the global CLF is
% Vi = x1^2 + (x1^2 + x2)^2 + x3^2
Vu(1) = x10^2 + (x10^2 + x20)^2 + x30^2;
V(1) = Vu(1);

% We introduce two variables allowing us to compute the switching time
% between the united controller and the local 'optimal' one. When the
% parameter is equal to 0 this means that we are before the switch.
OvSwitch1 = 0;
OvSwitch2 = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Integration of the model
for ii=1: NbIter-1

    %%%%%%%%%%%%%%%% Integration when using the usual global contoller
    x =  xvu(:,ii);
    x1 = x(1);
    x2 = x(2);
    x3 = x(3);
    % The global controller
    uu = -x1^2-x1-x3;
    % Integration of the model
    dx1dt = -x1 + x3;
    dx2dt = x1^2 -x2 -2*x1*x3 + x3;
    dx3dt = -x2 + uu;
    dxdt = [dx1dt; dx2dt; dx3dt];
    xvu(:,ii+1) = x + dxdt*dt;      
    Vu(ii+1) = x1^2 + (x1^2 + x2)^2 + x3^2;
    
    % Computation of the cost
    dJdtu = x'*Q*x +R*uu^2;
    Ju(ii+1) = Ju(ii) + dJdtu * dt; 
    
    %%%%%%%%%%%%%%%% Integration when using the united contoller
    x = xv(:,ii);
    x1 = x(1);
    x2 = x(2);
    x3 = x(3);
    % Computation of LgV     
    % First we compute
    % LgV0 = 2x'P0G
    % LgVi = x3
    LgV0 = 2*x'*P0*G;
    LgVi = 2*x3;


    % The global controler is :
    Phii = -x1^2-x1-x3;
    % The local one is -LgV0 / R
    Phi0 = -K0*x;
    
    V0 = x'*P0*x;
    Vi = x1^2 + (x1^2 + x2)^2 + x3^2;
    % we have
    % LgV = A LgV0 + B LgVi
    % with 
    % A = [R0 Vi - ri x'P0x] vphi0'(V0) + ri[1 - vphi0(V0) - vphii(Vi)] 
    % B = [R0 Vi - ri x'P0x] vphii'(Vi) + R0[vphi0(V0) + vphii(Vi)]
    % with 
    if V0 < r0
        vphi0=0;
        vphi0p=0;
    else if V0 > R0
            vphi0 = 1/2;
            vphi0p = 0;
        else
            vphi0 = 3/2 * ((V0-r0)/(R0-r0))^2 - ((V0-r0)/(R0-r0))^3;
            vphi0p = 3 / (R0-r0) * ((V0-r0)/(R0-r0) - ((V0-r0)/(R0-r0))^2);
        end
    end
    
    if Vi < ri
        vphii=0;
        vphiip=0;
    else if Vi > Ri
            vphii = 1/2;
            vphiip = 0;
        else
            vphii = 3/2 * ((Vi-ri)/(Ri-ri))^2 - ((Vi-ri)/(Ri-ri))^3;
            vphiip = 3 / (Ri-ri) * ((Vi-ri)/(Ri-ri) - ((Vi-ri)/(Ri-ri))^2);   
        end
    end
    %
    A = [R0 *Vi - ri *V0] *vphi0p + ri*[1 - vphi0 - vphii];
    B = [R0 *Vi - ri *V0] *vphiip + R0*[vphi0 + vphii];
    LgV = A * LgV0 + LgVi * B;
    %
    % The control law is given as :
    % gamma(x)phi0(x) + (1-gamma(x))phii - k * c(x)* LgV
    % with gamma defined as :
    gamma = min(1, max((Ri-Vi)/(Ri-ri),0));
    cx = max(0, (R0-V0)*(Vi-ri));
    u = gamma*Phi0 + (1-gamma)*Phii-k*cx*LgV;
    
    % Integration of the system with the control
    dx1dt = -x1 + x3;
    dx2dt = x1^2 -x2 -2*x1*x3 + x3;
    dx3dt = -x2 + u;
    dxdt = [dx1dt; dx2dt; dx3dt];
    xv(:,ii+1) = x + dxdt*dt;    
    V(ii+1) =  x1^2 + (x1^2 + x2)^2 + x3^2;
 
    % Computation of the cost
    dJdt = x'*Q*x +R*u^2;
    J(ii+1) = J(ii) + dJdt * dt; 
    
    
    
    % We check if we have switch the controler
    if (uu ~= u) & (OvSwitch1 == 0)
        OvSwitch1 = 1;
        ii1 = ii;
    end
    
    % We check if we are using the local controller and store the first
    % time of its use
    if (gamma == 1) & (OvSwitch2 == 0)
        OvSwitch2 = 1;
        ii2 = ii ;
    end
    
    Vect_u(ii) = u;
end

% Vector of time    
t = zeros(NbIter,1);
% Vector which specifies the line Vi = Ri and Vi = ri
vRi = zeros(NbIter, 1);
vri = zeros(NbIter, 1);
for ii=1: NbIter
    t(ii)=(ii-1)*dt;
    vRi(ii) = Ri;
    vri(ii) = ri;
end



% Plot of the evolution of the Cost in the two cases
plot(t,J,'-',t, Ju,'--', 'LineWidth',2);
hold on;
plot([t(ii1);t(ii1)],[0; J(ii1)],'--r','LineWidth',1.5);
hold on;
plot([t(ii2);t(ii2)],[0; J(ii2)],'--r','LineWidth',1.5);
xlabel('E','position', [1.50, -0.35, 1]);
ylabel('F');
text(t(ii1),J(ii1),'A',...
     'HorizontalAlignment','left')
 text(t(ii2),J(ii2),'B',...
     'HorizontalAlignment','left')
h_legend=legend('C','DDDDDDDDD', 4);
set(h_legend,'FontSize',20);
print -depsc2 'CostEvolution.eps'


% Plot of the evolution of the united control
figure;
plot(t,Vect_u, 'LineWidth',2);
hold on;
plot([t(ii1);t(ii1)],[0;Vect_u(ii1)],'--r','LineWidth',1.5);
hold on;
plot([t(ii2);t(ii2)],[0; Vect_u(ii2)],'--r','LineWidth',1.5);
text(t(ii1),Vect_u(ii1),'A',...
     'HorizontalAlignment','left')
 text(t(ii2),Vect_u(ii2),'B',...
     'HorizontalAlignment','left');
hold on;
plot(t,zeros(NbIter,1),'-','LineWidth',1);
xlabel('E','position', [1.50, -3.35, 1]);
ylabel('F');
print -depsc2 'ControlEvolution.eps'
