%%%%%  M file corresponding to the numerical example of the TAC paper about uniting CLF
%%%%%
%%%%%  DATE : 12th of March 2010

clear all;
close all;

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


% 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;

% Length of the simulation
Length = 5;

% Integration step size
dt = 0.01;

% Number of iteration per integration
NbIter = Length / dt;

% Number of different ball considered
NbBall = 15;

% Since we want an average of the cost we consider for instance the set of
% point such that : x1^2 + x2^2 + x3^2 = rr^2 in polar coordinates.
% The set of initial values of the model depends of a parameter NbInit
% which characterizes the number of initial point (namely NbInit^2)
NbInit = 15;

% les rayon considérés sur la fonctions sont tout les Pas_rr, i.e. RR(1) =
% Pas_rr, RR(2) = 2*Pas_rr...
Pas_rr = 0.18;
for bb = 1: NbBall
    % Initialisation of the matrix of initial condition
    VInit = zeros(NbInit^2,3);
    rr = Pas_rr * bb;
    for ii=1:NbInit
        for jj=1:NbInit
            phii = ii*2*pi/NbInit;
            thetaa = jj * pi/NbInit;
            x1 = rr * cos(phii) * cos(thetaa);
            x2 = rr * cos(phii) * sin(thetaa);
            x3 = rr * sin(phii);
            VInit((ii-1)*NbInit + jj,1)=real(x1);
            VInit((ii-1)*NbInit + jj,2)=real(x2);
            VInit((ii-1)*NbInit + jj,3)=real(x3);
        end
    end
    NbVal = NbInit^2;
    
    % Variable permettant de comptabiliser le nombre de fois que le united
    % controler est plus performant que le global
    Perfg = 0;
    Perfu = 0;
    
    
    % For each initial valu, we will compute the final cost by integrating the
    % model with and without the uniting controler
    for jj=1: NbVal
        % Initial values of the system considered. xg is the state for the
        % nominal global controller. Whereas xu is the state for the uniting
        % controller.
        x1u = VInit(jj,1);
        x1g = x1u;
        x2u = VInit(jj,2);
        x2g = x2u;
        x3u = VInit(jj,3);
        x3g = x3u;
        xu = [x1u; x2u; x3u];
        xg = [x1g; x2g; x3g];
        
        % Initialisation of the cost
        Ju = 0;
        Jg = 0;   
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Integration of the model
        for ii=1: NbIter-1
            
            %%%%%%%%%%%%%%%% Integration when using the nominal global contoller
            x1 = xg(1);
            x2 = xg(2);
            x3 = xg(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];
            xg = xg + dxdt*dt;      
            
            % Computation of the cost for the nominal global controller
            dJdt = xg'*Q*xg +R*uu^2;
            Jg = Jg + dJdt * dt; 
            
            %%%%%%%%%%%%%%%% Integration when using the united contoller
            x1 = xu(1);
            x2 = xu(2);
            x3 = xu(3);
            % Computation of LgV     
            % First we compute
            % LgV0 = 2x'P0G
            % LgVi = 2x3
            LgV0 = 2*xu'*P0*G;
            LgVi = 2*x3;
            
            
            % The global controler is :
            Phii = -x1^2-x1-x3;
            % The local one is -LgV0 / R
            Phi0 = -K0*xu;
            
            V0 = xu'*P0*xu;
            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];
            xu = xu + dxdt*dt;    
            
            % Computation of the cost
            dJdt = xu'*Q*xu +R*u^2;
            Ju = Ju + dJdt * dt; 
            
        end
        % If the cost is larger for the united controler then for the nominal
        % global one, then we store this information.
        if Ju > Jg
            Perfg = Perfg + 1;
        end
        % When this is the opposite, we also store this information.
        if Jg > Ju
            Perfu = Perfu + 1;
        end
        
    end
    % the united controller approach has given better results in Perfu% of the
    
    % initial value on the considered sphere.
    
    RR(bb) = bb*Pas_rr;
    Prop(bb) = Perfu / (Perfu + Perfg) * 100;
end

% for the plot we draw the line corresponding to the radius of the ball for
% which are sure to be inside the set Vi\leq ri. In other word, the line
% starting at. We make the following approximation
% Vi \leq 2(x'Pix)^2 + 2x'Pix
radius0=sqrt((-1+sqrt(1+2*ri))/2);
% Note that if x'Pi'x \leq radius0^2 then Vi \leq ri

plot(RR, Prop,'LineWidth',1.5);
xmin = min(RR); 
xmax =max(RR);
axis([ xmin xmax 0 100])
xlabel('E','position',[Pas_rr*NbBall/2, -8.87, 1]);
ylabel('F');
hold on;
plot([radius0;radius0],[0; 100],'--r','LineWidth',1.5);
hold on;
text(radius0,50,'A',...
     'HorizontalAlignment','left')
print -depsc2 'BenchInitCond.eps'
