%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%  M-File corresponding to the exemple in the paper

%%% "Nonlinear and locally optimal controllers design for input affine

%%% systems" submitting for publication at the IFAC world congress 2011.

%%%

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

%%%

%%%  Date 20/09/2010

%%

function dx=mod_union(t,x)

global b P0 Pinf Q R Rinf rinf R0 r0 k g l M m

% Changement de variable

v1=x(2)+2*[x(4)/sqrt(1+x(3)^2)]+x(3);

r2=x(4);

t2=x(3);

v2=v1;

% x(1)=x(3)/l;

x1=x(1)+2*log(x(3)+sqrt(1+x(3)^2));

x2=x1+10*v1+x(2)+x(4)/sqrt(1+x(3)^2);

% Loi de commande global obtenue par forwarding

Uinf0=2*x(3)*(1+x(4)^2/[(1+x(3)^2)]^(3/2))+x(4);

Uinf1=(1/10)*v1;

Uinf2=2*[(2*r2+t2)*sqrt(1+t2^2)+v2*(10+(1/2)*abs(v2))]+10*x2/sqrt(1+x2^2);

Uinf=(Uinf0+Uinf1+Uinf2);

%uin=Uinf*g/l*(M+m*(sin(x(1)))^2)-[m*l*x(2)^2*sin(x(1))-m*g*sin(x(1))*cos(x(1))];

   %CLF

V_0=x(4)^2+[((1+x(3)^2)^(3/2))-1]+x(4)*x(3);

V1=V_0+5*v1^2+(1/6)*abs(v1)^3;

Vinf=2*V1+[sqrt(1+x2^2)-1];

% Loi de commande optimale

U0=-inv(R)*b'*P0*x;

% Fonction de Lyapunov associée

V0=x'*P0*x;

%Dérivée de Lie de V0

LgV0 = 2*x'*P0*b;

%Dérivée de Lie de Vinf

LgV1=2*[10*v1+0.5*(abs(v1))^2]+11*x2/sqrt(1+(x2)^2)-1;

LgV2=-sqrt(1+t2^2)*[2*[2*r2+t2+20*v1/sqrt(1+(t2)^2)+(abs(v1))^2/sqrt(1+(t2)^2)]+21*x2/(sqrt(1+t2^2)*sqrt(1+x2^2))-1];

LgVinf=LgV1+LgV2;

%%%%%%%%%%%%%%%%%%%% Structure of V

                        if [(V0<r0)||(V0==r0)]

                            phi_0=0;

                            phi_0p =0;

                        else if [(V0>R0)||(V0==R0)]

                            phi_0=0.5;

                            phi_0p =0;

                        else

                            phi_0=3/2*[(V0-r0)/(R0-r0)]^2 - [(V0-r0)/(R0-r0)]^3;

                            phi_0p = 3/(R0-r0) * ((V0-r0)/(R0-r0) - ((V0-r0)/(R0-r0))^2);

                        end

                    end

                        if [(Vinf<rinf)||(Vinf==rinf)]

                            phi_inf=0;

                            phi_infp =0;

                        else if [(Vinf>Rinf)||(Vinf==Rinf)]

                            phi_inf=0.5;

                            phi_infp =0;

                        else

                            phi_inf=3/2*[(Vinf-rinf)/(Rinf-rinf)]^2-[(Vinf-rinf)/(Rinf-rinf)]^3;

                            phi_infp=3/(Rinf-rinf)*((Vinf-rinf)/(Rinf-rinf)-((Vinf-rinf)/(Rinf-rinf))^2);

                        end

                    end

                        V=R0*[phi_0+phi_inf]*Vinf+rinf*[1-phi_0-phi_inf]*V0;

                        A = [R0*Vinf - rinf*V0] *phi_0p + rinf*[1 - phi_0p - phi_infp];

                        B = [R0*Vinf - rinf*V0] *phi_infp + R0*[phi_0p + phi_infp];

                        LgV = A*LgV0 + LgVinf*B;

%%%%%%%%%%%%%%%%%Structure of the controller

                            c=max(0,(R0-V0)*(Vinf-rinf));    

                            %gamma=min(1,max((Rinf-Vinf)/(Rinf-rinf),0));

                            gamma=1-phi_0-phi_inf;

                            phi=gamma*U0+(1-gamma)*Uinf-k*c*LgV;

%%%%%%%%%%%%%%%%%%%%%%%%%%% Le modele

xdot=[x(2) phi x(4)  2*x(3)*x(4)^2/(1+x(3)^2)+x(3)*sqrt(1+x(3)^2)-phi*sqrt(1+x(3)^2)]';

dx=xdot;