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