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