Skip to content
Snippets Groups Projects
symb_sim.m 1.34 KiB
Newer Older
Girard Antoine's avatar
Girard Antoine committed
, %% Tutorial example on symbolic control
%% Author: Antoine GIRARD, Universit Paris-Saclay, CNRS, CentraleSuplec, Laboratoire des signaux et systmes, 91190, Gif-sur-Yvette, France.
%% Date: 2024

% Simulation of closed-loop trajectories

clear 

load('symb_synth.mat');

figure(3)
hold on
fill([R1(1,1) R1(1,2) R1(1,2) R1(1,1)],...
     [R1(2,1) R1(2,1) R1(2,2) R1(2,2)],'y','FaceAlpha',0.4);
fill([R2(1,1) R2(1,2) R2(1,2) R2(1,1)],...
     [R2(2,1) R2(2,1) R2(2,2) R2(2,2)],'c','FaceAlpha',0.4);
fill([R3(1,1) R3(1,2) R3(1,2) R3(1,1)],...
     [R3(2,1) R3(2,1) R3(2,2) R3(2,2)],'g','FaceAlpha',0.4);
fill([R4(1,1) R4(1,2) R4(1,2) R4(1,1)],...
     [R4(2,1) R4(2,1) R4(2,2) R4(2,2)],'r','FaceAlpha',0.4);


num_step=100;

x0=ones(3,50).*[6.8;9;0];
%x0=[[0.5;4.5;0],[0.5;4;0]]; 
 
for i=1:size(x0,2)

x=x0(:,i);
psi=I_s;
z=x;
Tx=[x];
for t=1:num_step 
   
   psi=h1(psi,q(x));
  
   
   if psi==F_s
       break;
   end
   u=p(h2(psi,q(x)));
   w=bound_w(:,1)+diag(rand(3,1))*d_w;
   x=f(x,u,w);
   z=f(z,u,w); 
   if x(3)>pi
       x(3)=x(3)-2*pi;
   elseif x(3)<-pi
       x(3)=x(3)+2*pi;
   end
   Tx=[Tx,z];
end
plot(Tx(1,:),Tx(2,:),'-o','color','r');
%plot(Tx(1,:),Tx(2,:),'-o');
end
hold off
axis([bound_x(1,1) bound_x(1,2) bound_x(2,1) bound_x(2,2)]);
ax = gca;
ax.FontSize = 16; 
xlabel('x_1');
ylabel('x_2');
title('Trajectories of the closed-loop system')