%%%%% M file corresponding to the numerical example of the TAC paper about uniting CLF %%%%% %%%%% DATE : 12th of March 2010 clear all; close all; % The system is % dot x1 = -x1 + x3 % dot x2 = x1^2 -x2 -2x1x3 + x3 % dot x3 = -x2 + u % 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; % Length of the simulation Length = 5; % Integration step size dt = 0.01; % Number of iteration per integration NbIter = Length / dt; % Number of different ball considered NbBall = 15; % Since we want an average of the cost we consider for instance the set of % point such that : x1^2 + x2^2 + x3^2 = rr^2 in polar coordinates. % The set of initial values of the model depends of a parameter NbInit % which characterizes the number of initial point (namely NbInit^2) NbInit = 15; % les rayon considérés sur la fonctions sont tout les Pas_rr, i.e. RR(1) = % Pas_rr, RR(2) = 2*Pas_rr... Pas_rr = 0.18; for bb = 1: NbBall % Initialisation of the matrix of initial condition VInit = zeros(NbInit^2,3); rr = Pas_rr * bb; for ii=1:NbInit for jj=1:NbInit phii = ii*2*pi/NbInit; thetaa = jj * pi/NbInit; x1 = rr * cos(phii) * cos(thetaa); x2 = rr * cos(phii) * sin(thetaa); x3 = rr * sin(phii); VInit((ii-1)*NbInit + jj,1)=real(x1); VInit((ii-1)*NbInit + jj,2)=real(x2); VInit((ii-1)*NbInit + jj,3)=real(x3); end end NbVal = NbInit^2; % Variable permettant de comptabiliser le nombre de fois que le united % controler est plus performant que le global Perfg = 0; Perfu = 0; % For each initial valu, we will compute the final cost by integrating the % model with and without the uniting controler for jj=1: NbVal % Initial values of the system considered. xg is the state for the % nominal global controller. Whereas xu is the state for the uniting % controller. x1u = VInit(jj,1); x1g = x1u; x2u = VInit(jj,2); x2g = x2u; x3u = VInit(jj,3); x3g = x3u; xu = [x1u; x2u; x3u]; xg = [x1g; x2g; x3g]; % Initialisation of the cost Ju = 0; Jg = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Integration of the model for ii=1: NbIter-1 %%%%%%%%%%%%%%%% Integration when using the nominal global contoller x1 = xg(1); x2 = xg(2); x3 = xg(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]; xg = xg + dxdt*dt; % Computation of the cost for the nominal global controller dJdt = xg'*Q*xg +R*uu^2; Jg = Jg + dJdt * dt; %%%%%%%%%%%%%%%% Integration when using the united contoller x1 = xu(1); x2 = xu(2); x3 = xu(3); % Computation of LgV % First we compute % LgV0 = 2x'P0G % LgVi = 2x3 LgV0 = 2*xu'*P0*G; LgVi = 2*x3; % The global controler is : Phii = -x1^2-x1-x3; % The local one is -LgV0 / R Phi0 = -K0*xu; V0 = xu'*P0*xu; 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]; xu = xu + dxdt*dt; % Computation of the cost dJdt = xu'*Q*xu +R*u^2; Ju = Ju + dJdt * dt; end % If the cost is larger for the united controler then for the nominal % global one, then we store this information. if Ju > Jg Perfg = Perfg + 1; end % When this is the opposite, we also store this information. if Jg > Ju Perfu = Perfu + 1; end end % the united controller approach has given better results in Perfu% of the % initial value on the considered sphere. RR(bb) = bb*Pas_rr; Prop(bb) = Perfu / (Perfu + Perfg) * 100; end % for the plot we draw the line corresponding to the radius of the ball for % which are sure to be inside the set Vi\leq ri. In other word, the line % starting at. We make the following approximation % Vi \leq 2(x'Pix)^2 + 2x'Pix radius0=sqrt((-1+sqrt(1+2*ri))/2); % Note that if x'Pi'x \leq radius0^2 then Vi \leq ri plot(RR, Prop,'LineWidth',1.5); xmin = min(RR); xmax =max(RR); axis([ xmin xmax 0 100]) xlabel('E','position',[Pas_rr*NbBall/2, -8.87, 1]); ylabel('F'); hold on; plot([radius0;radius0],[0; 100],'--r','LineWidth',1.5); hold on; text(radius0,50,'A',... 'HorizontalAlignment','left') print -depsc2 'BenchInitCond.eps'