Upload New File

symb_synth.m

+%% 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
+
+% Synthesis of a controller for the specification defined in specification.m
+
+clear 
+
+load('symb_abs.mat');
+
+specification;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Symbolic controller synthesis
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+tic
+
+disp('Step 2: Symbolic controller synthesis')
+
+% Labeling the states
+Lab_xi=zeros(1,n_sigma);
+for xi=1:n_xi   
+    c_xi=state2coord(xi,p_x);         
+    x_c=bound_x(:,1)+(c_xi-0.5).*d_x; % Coordinates of the center of the cell
+    Lab_xi(xi)=Lab_x(x_c);
+end
+
+% Choice of a sucessor sample
+g_p=zeros(n_s,n_xi,n_sigma);
+for psi=1:n_s
+    g_p(psi,:,:)=g(:,:,1);
+end
+
+%Initialize the value function
+V=inf*ones(n_s,n_xi);
+V(F_s,:)=0; % final states
+
+%Initialize the controller
+h1=@(psi,xi) A_s(psi,Lab_xi(xi));
+h2=zeros(n_s,n_xi);
+
+iter=0;
+fixpoint=0;
+
+while ~fixpoint  
+    iter=iter+1;
+    fprintf('Fixed point algorithm: iteration %d \n',iter);
+    Vp=V;
+    
+    for xi=1:n_xi
+        for psi=1:n_s 
+            psi_succ=h1(psi,xi);
+            if V(psi,xi)==inf
+               if h2(psi_succ,xi)~=0
+                  V(psi,xi)=iter; 
+               else
+                  Vmax=zeros(1,n_sigma); 
+                  for sigma=1:n_sigma
+                      xi_succ=g(xi,sigma,:);
+                      if all(xi_succ) % the input is enabled 
+                         if (Vp(psi_succ,g_p(psi,xi,sigma))~=inf) % the successor sample is controllable
+                            c_min=state2coord(xi_succ(1),p_x);
+                            c_max=state2coord(xi_succ(2),p_x);
+                            if c_min(3)<=c_max(3)
+                               Q_succ=coordbox2state(c_min,c_max,grid_xi)' ;           
+                            else 
+                               %Special treatment of the third coordinate (angle) 
+                               %not needed in general
+                               Q_succ=[coordbox2state(c_min,[c_max(1);c_max(2);n_x(3)],grid_xi)',...
+                                       coordbox2state([c_min(1);c_min(2);1],c_max,grid_xi)'];
+                            end
+                            V_succ=Vp(psi_succ,Q_succ);   % list of all successors
+                            Vmax(sigma)=all(V_succ~=inf); % check if all successors are controllable
+                            if Vmax(sigma)
+                                break;
+                            else
+                               [m,i_p]=max(V_succ~=inf);
+                               g_p(psi,xi,sigma)=Q_succ(i_p);
+                            end
+                         end
+                      end
+                  end
+                  if any(Vmax)
+                     % controllable if there exists an input such that all sucessors are controllable 
+                     V(psi,xi)=iter; 
+                     [m,sigma]=max(Vmax);
+                     h2(psi_succ,xi)=sigma;
+                  end
+               end
+            end
+        end
+    end
+    fixpoint=isequal(Vp,V);
+end
+disp('Fixed point reached')
+
+toc
+
+save('symb_synth.mat');
\ No newline at end of file