%%%%%  M file corresponding to one of the numerical example of the

%%%%%  IFAC-WorldCongress paper entitled "Nonlinear and locally optimal

%%%%%  controllers design for input affine systems.

%%%%%

%%%%%  In this M-file, we test the algorithm presented in Section 5. It

%%%%%  requires the solver Sedumi and Yalmip to be installed.

%%%%%

%%%%%  author : M. Sahnoun, V. Andrieu, M. Nadri

%%%%%

%%%%%  DATE of creation : 11/8/2010 -- Updated 17/09/2010 (V. Andrieu)

%%%%%

clear all;

close all;

% The first order approximation of the nonlinear system describing the

% inverted pendulum is given by

% dot x = A x + B u with

A = [[0 1 0 0];[0 0 0 0];[0 0 0 1];[0 0 1 0]];

B = [0;1;0;-1];

% For this system a CLF can be designed threw Forwarding design (following

% Praly and Mazenc). Note that the quadratic approximation of this CLF is

% x^TPix wit

Pi=[

    [0.5 5.5 6 21/2];

    [5.5 141/2 76 271/2];

    [6 76 85 147];

    [21/2 271/2 147 525/2]

    ];

j=0;

n=0;

maxl=0;

NbTry=1000; % Corresponds to the number of test for the local controller

NbTry2=40; % corresponds to the limit max of test we make given a random Pm

for i=1:NbTry

    % We design a LQ controller    

    % Parameter of the LQ controller

    q = rand(4,4);

    Q =  q'*q;

    R = 1;

    % We solve the riccatti equation

    [P0, L, K0, rr] = care(A, B, Q, R, zeros(4,1), eye(4));

    % The local controler phi0 is given as u = -B'*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,4);

    % Matrix that has to be negative for the local design

    Mat0 = [P0*(A+B*K) + (A+B*K)'*P0];

    Mati = [Pi*(A+B*K) + (A+B*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 was solved

        j=j+1;

    else

        % In that the LMI was not solved we will try to use the extension

        % with a transitive CLF

        l=0;

        while l<NbTry2

            l=l+1;

            % Parameter of the LQ controller for the transitive CLF.

            % Following the algorithm this one is selected randomly.

            q2 = rand(4,4);

            Q2 =  q2'*q2;

            R2 = 1;

            [Pm, L, KK, rrr] = care(A, B, Q2, R2, zeros(4,1), eye(4));

            Km1 = sdpvar(1,4);

            Km2 = sdpvar(1,4);

            Mat1 = [P0*(A+B*Km1) + (A+B*Km1)'*P0];

            Mat2 = [Pm*(A+B*Km1) + (A+B*Km1)'*Pm];

            Mat3 = [Pm*(A+B*Km2) + (A+B*Km2)'*Pm];

            Mat4 = [Pi*(A+B*Km2) + (A+B*Km2)'*Pi];

            LMI_ext1 =  [Mat1<0, Mat2<0];

            LMI_ext2 =  [Mat3<0, Mat4<0];

            solvesdp(LMI_ext1, [], opt);

            solvesdp(LMI_ext2, [], opt);

             % we check if the LMI was solved

            if max([max(eig(double(Mat1))), max(eig(double(Mat2))), max(eig(double(Mat3))), max(eig(double(Mat4)))])<0

                % In that case the LMI was solved

                % We update the number of case in which our approach works

                n=n+1;

                if l>maxl

                    %we evaluate the maximal number of test we had to make

                    %to solve the LMI

                    maxl=l;

                end

                l=NbTry2;

            end

        end

    end

end

% Purcentage of succes without employing the extension

disp(['Pourcentage of success without extension : ',num2str(j/NbTry*100),'%'])

disp(['Pourcentage of success employing the extension : ',num2str((j+n)/NbTry*100),'%'])