%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 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
%%
close all
clear all
clc
tic
global b P0 Pinf Q R Rinf rinf R0 r0 k g l M m %phi_Vect Uinf_Vect Vinf_Vect U0_Vect V0_Vect
k=10;
M=0.2;
m=0.1;
l=0.26;
g=9.81;
s=0;
% approximation du premier ordre du système
a=[0 1 0 0;0 0 0 0;0 0 0 1;0 0 1 0];
b=[0 1 0 -1]';
p=0.001; % pas d'intégration
t=0:.01:25; % intervalle du temps
%X0=[0.07 0.03 0.05]'; % Valeur intial
X0=[10 .1 1.73 1]';
%Paramètre du Controlleur LQ
R=1;
%Q=eye(4);
% Q=[1.5605 1.3089 1.6415 1.3505
% 1.3089 1.5839 1.3666 1.4722
% 1.6415 1.3666 1.8783 1.5152
% 1.3505 1.4722 1.5152 1.5903];
% Q;
Q=[0.1 0 0 0
0 0.2 0 0
0 0 0.3 0
0 0 0 0.4];
Q
Pinf=[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]
% approximaion quadratique de CLF forwarding
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Résoudre equation de Ricatti
P0=are(a,b*b',Q)
-R^(-1)*b'*P0
%P0=Pinf;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%resolution de LMI
% lmiedit
setlmis([]);
k0=lmivar(2,[1,4]);
lmiterm([1 1 1 -k0],.5*1,b'*P0,'s'); % LMI #1: k0'*b'*P0 (NON SYMMETRIC?)
lmiterm([1 1 1 k0],.5*P0*b,1,'s'); % LMI #1: P0*b*k0 (NON SYMMETRIC?)
lmiterm([-1 1 1 0],-(a'*P0+P0*a)); % LMI #1: -(a'*P0+P0*a)
lmiterm([2 1 1 -k0],.5*1,b'*Pinf,'s'); % LMI #2: k0'*b'*Pinf (NON SYMMETRIC?)
lmiterm([2 1 1 k0],.5*Pinf*b,1,'s'); % LMI #2: Pinf*b*k0 (NON SYMMETRIC?)
lmiterm([-2 1 1 0],-(a'*Pinf+Pinf*a)); % LMI #2: -(a'*Pinf+Pinf*a)
teste=getlmis;
options_feasp = zeros(5,1);
% options_feasp(3)=1e6;
[tmin,xfeas] = feasp(teste, options_feasp, -1);
k0=xfeas'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%initialisation des vecteurs de stockage
Uinf_Vect=[];% stockage des valeurs de la commande global;
Vinf_Vect=[]; % stockage des valeurs de la CLF global;
U0_Vect=[]; % stockage des valeurs de la commande local;
V0_Vect=[];% stockage des valeurs de la CLF local;
phi_Vect=[]; % stockage des valeurs de la commande union phi;
Vt=[]; % stockage des valeurs de la CLF union;
te=[];tq=[];
J1_Vect=[]; % stockage des valeurs de la fonction de cout de phi;
J2_Vect=[]; % stockage des valeurs de la fonction de cout de forwarding;
J1=0;J2=0;Jb=0;
eps = 0.5;
% On veut que rinf x'P0 x < R0 x'Pinf x,
rinf = eps * 0.7 * min(eig(Pinf))
R0 = eps * 1.3 * max(eig(P0))
% et rinf x'P0x \leq r0 x'Pix,
r0 = rinf * max(eig(P0)) / min(eig(Pinf))
% et Rinf x'P0 x \leq R0 x'Pi x,
Rinf = R0 * min(eig(Pinf)) / max(eig(P0))
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% l''etat du modèle de l''union
[t,x_1]=ode15s(@mod_union,t,X0);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% stockage des valeurs des commandes pour le traçage
for i=1:length(t)
% Changement de variable
v1=x_1(i,2)+2*[x_1(i,4)/sqrt(1+x_1(i,3)^2)]+x_1(i,3);
r2=x_1(i,4);
t2=x_1(i,3);
v2=v1;
% x(i,1)=x(i,3)/l;
x1=x_1(i,1)+2*log(x_1(i,3)+sqrt(1+x_1(i,3)^2));
x2=x1+10*v1+x_1(i,2)+x_1(i,4)/sqrt(1+x_1(i,3)^2);
% Loi de commande global obtenue par forwarding
Uinf0=2*x_1(i,3)*(1+x_1(i,4)^2/[(1+x_1(i,3)^2)]^(3/2))+x_1(i,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(i,1)))^2)-[m*l*x_1(i,2)^2*sin(x_1(i,1))-m*g*sin(x_1(i,1))*cos(x_1(i,1))];
Uinf_Vect=[Uinf_Vect Uinf];
%CLF
V_0=x_1(i,4)^2+[((1+x_1(i,3)^2)^(3/2))-1]+x_1(i,4)*x_1(i,3);
V1=V_0+5*v1^2+(1/6)*abs(v1)^3;
Vinf=2*V1+[sqrt(1+x2^2)-1];
Vinf_Vect=[Vinf_Vect Vinf];
% fonction de la commande local
U0=-inv(R)*b'*P0*[x_1(i,:)]';
U0_Vect=[U0_Vect U0];
% CLF local
V0=x_1(i,:)*P0*[x_1(i,:)]';
V0_Vect=[V0_Vect V0];
% Dérivée de lie de VO
LgV0 = 2*[x_1(i,:)]*P0*b;
% Dérivée de lie de Vinf
% LgV1=20*x(i,2)+40*r2/(1+t2^2)^(1/2)+20*t2+abs(x(i,2)+2*r2/(1+t2^2)^(1/2)+t2)^2*abs(x(i,2)+2*r2/(1+t2^2)^(1/2)+t2)+1/2/(1+(x(i,1)+2*log(t2+(1+t2^2)^(1/2))+11*x(i,2)+21*r2/(1+t2^2)^(1/2)+10*t2)^2)^(1/2)*(22*x(i,1)+44*log(t2+(1+t2^2)^(1/2))+242*x(i,2)+462*r2/(1+t2^2)^(1/2)+220*t2);
LgV1=2*[10*v1+0.5*(abs(v1))^2]+[11*x2/sqrt(1+(x2)^2)]-1;
% LgV2=-sqrt(1+t2^2)*(4*r2+2*t2+40*(x(i,2)+2*r2/(1+t2^2)^(1/2)+t2)/(1+t2^2)^(1/2)+2*abs(x(i,2)+2*r2/(1+t2^2)^(1/2)+t2)^2*abs(x(i,2)+2*r2/(1+t2^2)^(1/2)+t2)/(1+t2^2)^(1/2)+21/(1+(x(i,1)+2*log(t2+(1+t2^2)^(1/2))+11*x(i,2)+21*r2/(1+t2^2)^(1/2)+10*t2)^2)^(1/2)*(x(i,1)+2*log(t2+(1+t2^2)^(1/2))+11*x(i,2)+21*r2/(1+t2^2)^(1/2)+10*t2)/(1+t2^2)^(1/2));
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 de 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);
te=[te i];
end
end
if [(Vinf<rinf)||(Vinf==rinf)]
phi_inf=0;
phi_infp=0;
tq=[tq i];
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; % CLF de l''union
Vt=[Vt V/R0]; % Pour comparer les fonctions de lyapunov avec le global on divise par R0
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 ;
phi_Vect=[phi_Vect phi];
dJdt=[x_1(i,:)]*Q*[x_1(i,:)]'+R*phi^2;
J1=J1+p*dJdt;
J1_Vect=[J1_Vect J1];
end
[t,x_2]=ode15s(@mod_for,t,X0);
U=[]; J=0; J_Vect=[];
for i=1:length(t)
v1=x_2(i,2)+2*[x_2(i,4)/sqrt(1+x_2(i,3)^2)]+x_2(i,3);
r2=x_2(i,4);
t2=x_2(i,3);
v2=v1;
% x(i,1)=x(i,3)/l;
x1=x_2(i,1)+2*log(x_2(i,3)+sqrt(1+x_2(i,3)^2));
x2=x1+10*v1+x_2(i,2)+x_2(i,4)/sqrt(1+x_2(i,3)^2);
% Loi de commande global obtenue par forwarding
Uinf0=2*x_2(i,3)*(1+x_2(i,4)^2/[(1+x_2(i,3)^2)]^(3/2))+x_2(i,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);
%u0=Uinf*g/l*(M+m*(sin(x_2(i,1)))^2)-[m*l*x_2(i,2)^2*sin(x_2(i,1))-m*g*sin(x_2(i,1))*cos(x_2(i,1))];
U=[U Uinf];
%CLF
V_0=x_2(i,4)^2+[((1+x_2(i,3)^2)^(3/2))-1]+x_2(i,4)*x_2(i,3);
V1=V_0+5*v1^2+(1/6)*abs(v1)^3;
Vinf(i)=2*V1+[sqrt(1+x2^2)-1];
%cout
djdt=[x_2(i,:)]*Q*[x_2(i,:)]'+R*Uinf^2;
J2=J2+p*djdt;
J2_Vect=[J2_Vect J2];
end
figure(1)
subplot(2,1,1), plot(t,x_1(:,1),'r'),hold on,plot(t,x_1(:,2),'m'),plot(t,x_1(:,3),'b'),plot(t,x_1(:,4),'y'),
LEGEND('La variation de la 1ère composante','La variation de la 2ème composante','La variation de la 3ème composante','La variation de la 4ème composante'),
xlabel('Temps[sec]'),ylabel('Modèle_1'),grid,title('L''évolution des états de phi')
subplot(2,1,2), plot(t,x_2(:,1),'r'),hold on,plot(t,x_2(:,2),'m'),plot(t,x_2(:,3),'b'),plot(t,x_2(:,4),'y'),
LEGEND('La variation de la 1ère composante','La variation de la 2ème composante','La variation de la 3ème composante','La variation de la 4ème composante'),
xlabel('Temps[sec]'),ylabel('Modèle_2'),grid,title('L''évolution des états de la loi de commande forwarding')
figure(2)
SUBPLOT(2,1,1), plot(t,U0_Vect,'--m','LineWidth',1),hold on,plot(t,Uinf_Vect,'-.b','LineWidth',1),plot(t,phi_Vect,':r','LineWidth',2),
xlabel('Temps[sec]'),ylabel('Commande'),LEGEND('Commande locale','Commande globale','Commande forwarding modifiée'),
% plot(t(te(1)),-30:0.5:Uinf_Vect(te(1)),'--k','LineWidth',1),plot(t(tq(1)),-30:0.5:U0_Vect(tq(1)),'--k','LineWidth',1),
% plot(t(te(1)):0.1:t(tq(1)),-20,'--k','LineWidth',1)
% plot(t(te(1)),-20,'<'),plot(t(tq(1)),-20,'>')
% text(t(tq(1)-te(1)),-15, 'Domaine d''interpolation')
% gtext('V_0(x(t))=R_0')
% gtext('V_inf(x(t))=r_inf')
%,plot(t(te(1)),Uinf_Vect(te(1)),'.r','LineWidth',3)
,title('L''évolution des commandes')
SUBPLOT(2,1,2), plot(t,V0_Vect,'--m','LineWidth',1),hold on,plot(t,Vinf_Vect,'b','LineWidth',1),plot(t,Vt,':r','LineWidth',2),
LEGEND('Lyapunov local','Lyapunov global','Lyapunov forwarding modifié'),ylabel('CLF'),xlabel('Temps[sec]'),
title('L''évolution des fonctions de Lyapunov'),
% plot(t(te(1)),Vinf_Vect(te(1)):0.5:70,'--k','LineWidth',1),plot(t(tq(1)),V0_Vect(tq(1)):0.5:70,'--k','LineWidth',1)
% plot(t(te(1)):0.1:t(tq(1)),50,'--k','LineWidth',1)
% plot(t(te(1)),50,'<'),plot(t(tq(1)),50,'>')
% text(20,60, 'Domaine d''interpolation')
%t(tq(1)-te(1))
% gtext('V_0(x(t))=R_0')
% gtext('V_inf(x(t))=r_inf')
figure(3)
subplot(2,1,1), plot(t,U,'-','LineWidth',1),xlabel('Temps[sec]'),ylabel('Phi_inf'),LEGEND('Commande du Forwarding')
subplot(2,1,2), plot(t,Vinf,'-','LineWidth',1), xlabel('Temps[sec]'),ylabel('V_inf'),LEGEND('Lyapunov du Forwarding')
%hold on, plot(t,Vt,'r')
figure(4)
plot(t,J1_Vect,':r','LineWidth',2),hold on, plot(t,J2_Vect,'b','LineWidth',1),
LEGEND('Fonction de cout de l''union','Fonction de cout du forwarding'),xlabel('Temps[sec]') ,ylabel('J')
% plot(t(te(1)),0:0.5:J1_Vect(te(1)),'--k','LineWidth',1)
% plot(t(tq(1)),0:0.5:J1_Vect(tq(1)),'--k','LineWidth',1)
title('L''évolution des fonctions du cout')
toc
% figure(5)
% plot(J_Vect-Jb_Vect,'b')
% xlabel('Temps')
% title('La différence entre les deux couts')
% grid