├── README.txt └── source_code ├── Agent.m ├── Autonomous Vehicles ├── Mstep_lookup_AV_MC_STO_1M_4D_26z0_unstable.mat ├── Mstep_lookup_AV_MC_STO_2M_4D_26z0_unstable.mat ├── Mstep_lookup_AV_MC_STO_3M_4D_26z0_unstable.mat ├── Mstep_lookup_AV_MC_STO_4M_4D_26z0_unstable.mat ├── Mstep_lookup_AV_MC_STO_5M_4D_26z0_unstable.mat ├── init_av_params.m ├── reset_lookup_AV_STO_1M_4D.mat ├── reset_lookup_AV_STO_2M_4D.mat ├── reset_lookup_AV_STO_3M_4D.mat ├── reset_lookup_AV_STO_4M_4D.mat ├── reset_lookup_AV_STO_5M_4D.mat └── setLeadVelocity.m ├── CP_compare_ETvPT_real_experiment.m ├── Mstep_probability.m ├── Mstep_probability_setup.m ├── Multi_Agent_Simulation.m ├── NetworkManager.m ├── NetworkManager_ET.m ├── getExitProbPDE.m ├── mergeFigures.m ├── multi_agent_setup.m ├── network_setup.m ├── params.m ├── plot_CACC_Comparison.m ├── plot_settings.m ├── run_Monte_Carlo.m ├── solve_PDE_1D.m ├── solve_PDE_2D.m └── solve_PDE_3D.m /README.txt: -------------------------------------------------------------------------------- 1 | ==================================================================================================== 2 | Predictive Triggering Framework for Distributed Control of Resource Constrained Multi-agent Systems 3 | ==================================================================================================== 4 | 5 | Source code for the paper "Predictive Triggering for Distributed Control of Resource Contrained 6 | Multi-agent Systems", submitted to the 2019 Distributed Estimation and Control in Networked Systems 7 | conference (NecSys 2019). The code (as is curremtly set up) simulates a Cooperative Adaptive Cruise 8 | Control (CACC) system for on a 5 lane highway, where each lane follows a reference vehicle. 9 | 10 | To run the CACC simulations, the user requires Matlab version R2010a or higher (due to class 11 | definitions) and have the Statistics and Machine Learning toolbox installed. A single simulation may 12 | be performed by running Multi_Agent_Simulation.m. The following are key files needed to modify/analyze 13 | the simulations: 14 | 15 | Multi_Agent_Simulation.m: 16 | Runs a single simulation using the parameters defined in params.m and 17 | the CACC vehicle model defined in multi_agent_setup.m 18 | 19 | params.m: 20 | Defines the simulation, trigger and network parameters needed to run the simulations. 21 | 22 | multi_agent_setup.m: 23 | Defines the multi-agent system parameters (ie., number of agents, models) 24 | 25 | Agent.m: 26 | Contains the class definition for a single agent, where the methods represnt the function 27 | blocks (ie., process, predictors, trigger) 28 | 29 | /Autonomous Vehicles: 30 | Folder which contains the lookup tables (currently for sim.delta = 0.01, 31 | sim.Q = diag([9e-6,9e-6,9e-6,9e-6])) and reference velocity generator function. 32 | 33 | **NOTE: if the sim.delta or the sim.Q parameters are changed, new lookup tables need to be computed. 34 | 35 | 36 | 37 | José Mario Mastrangelo 38 | MPI-IS, ICS 39 | josema(at)student.ethz.ch; mastrangelo(at)is.mpg.de 40 | 41 | Copyright 2019 Max Planck Society. All rights reserved. 42 | ==================================================================================================== -------------------------------------------------------------------------------- /source_code/Agent.m: -------------------------------------------------------------------------------- 1 | % Agent class containing the fields and methods needed to run the ET, PT,ST 2 | % blocks 3 | 4 | classdef Agent < handle 5 | 6 | properties 7 | ID % agent identification number 8 | trig_type % triggering type 9 | A % process model of current agent [nx x nx] 10 | Ahat % prediction models of all agents [nx x nx x N] 11 | B % input matrix [nx x nu] 12 | Bff % feedforward matrix (for AV) [nx x 1] 13 | x0 % initial state of current agent [nx x 1] 14 | x % current state of current agent [nx x T] 15 | xhat % predicted states of all agents [nx x T x N] 16 | z % estimation error of agent [nx x T] 17 | z_curr % current error of agent [nx x 1] 18 | u % control input for process [nu x T] 19 | e % control error of agent [nx x T] 20 | lane % lane of vehicle [1 x 1] 21 | pLane % vehicle position in lane [1 x 1] 22 | a % vehicle acceleration [1 x T] 23 | v % vehicle velocity [1 x T] 24 | px % vehicle x-position [1 x T] 25 | py % vehicle y-position [1 x T] 26 | d % distance to preceding [1 x T] 27 | dr % desired distance to preceding [1 x T] 28 | se % (AV) spacing error [1 x T] 29 | ppreced % preceding vehicles position [1 x T] 30 | psent % position sent at time k [1 x 1] 31 | ad % desired acceleration [1 x T] 32 | chi % control input for AV [nu x T] 33 | adtilde % desired acc. of i-1 seen by i [1 x T] 34 | F % controller gain [nu x nx] 35 | H % 1-M step exit probability from Z(k) [M x 1] 36 | M % prediction horizon (variable for ST) 37 | comm_prob % comm prob in prediction horizon [1 x T] 38 | comm_prob_mstep % M-step communication probability 39 | comm_slot % assigned communication slot [T x {0,1}] 40 | comm_slot_mstep % M-step slot assignment [{0,1}] 41 | pot_comm_slot % potential comm slot for ST [T x {0,1}] 42 | comm_trig % communication trigger [T x {0,1}] 43 | comm_trig_curr % current triggering decision [{0,1}] 44 | comm_agents % comm decision of all agents at time k [{0,1} x N] 45 | sent_state % state sent by current agent comm [nx x 1] 46 | all_sent_states % states sent by all agents at time k [nx x N] 47 | end 48 | 49 | methods 50 | % Constructor 51 | function obj = Agent(sim,ID) 52 | if(nargin > 0) 53 | if(isnumeric(sim.A) && isnumeric(sim.Ahat) && isnumeric(sim.x0)) 54 | if(size(sim.x0,1) == length(sim.A)) 55 | obj.x0 = sim.x0(:,:,ID); 56 | obj.x = zeros(sim.nx,length(sim.t)); 57 | obj.x(:,1) = obj.x0; 58 | else 59 | error('Initial state must be of length n'); 60 | end 61 | if(size(sim.A) == size(sim.Ahat(:,:,1))) 62 | obj.A = sim.A; 63 | obj.Ahat = sim.Ahat; 64 | obj.xhat = zeros(sim.nx,length(sim.t),sim.N); 65 | obj.xhat(:,1,:) = sim.x0; 66 | else 67 | error('Process and prediction models must be the same size') 68 | end 69 | else 70 | error('Inputs must be numeric matrices') 71 | end 72 | if(strcmp(sim.PROB_TYPE,'CONTROL')) 73 | obj.B = sim.B; 74 | obj.e = zeros(sim.nx,length(sim.t)); 75 | if(strcmp(sim.SIM_TYPE,'AV')) 76 | obj.lane = obj.getLane(sim.posy(ID,1),sim.lwidth); 77 | obj.pLane = ID - (obj.lane-1)*sim.Nl(obj.lane); 78 | obj.a = sim.a(ID,:); 79 | obj.v = sim.v(ID,:); 80 | obj.px = sim.posx(ID,:); 81 | obj.py = sim.posy(ID,:); 82 | obj.d = sim.d(ID,:); 83 | obj.dr = sim.dr(ID,:); 84 | obj.se = sim.e(ID,:); 85 | obj.ad = sim.u(ID,:); 86 | 87 | obj.Bff = sim.Bff; 88 | obj.F = sim.K; 89 | obj.chi = zeros(sim.nu,length(sim.t)); 90 | 91 | % Q = 0.1*eye(sim.nx); 92 | % R = 0.01*eye(sim.nu); 93 | % Qn = diag([sim.var2 sim.var3 sim.var2 sim.var3]); 94 | % 95 | % Rn = zeros(sim.nx); 96 | % sys = ss(obj.A,obj.B,eye(sim.nx),zeros(sim.nx,1),sim.dt); 97 | % QXU = blkdiag(Q,R); 98 | % QWV = blkdiag(Qn,Rn); 99 | % Klqg = lqg(sys,QXU,QWV); 100 | % 101 | % obj.F = Klqg.C; 102 | obj.psent = 0; 103 | if(mod(ID,sim.Nl(obj.lane)) == 1) 104 | obj.adtilde(1) = sim.uref(obj.lane,1); 105 | obj.chi(:,1) = obj.F(1:3)*obj.xhat(1:3,1,ID) + obj.adtilde(1); 106 | else 107 | obj.adtilde(1) = sim.uref(obj.lane,1); 108 | obj.chi(:,1) = obj.F(1:3)*obj.xhat(1:3,1,ID) + obj.adtilde(1); 109 | end 110 | 111 | elseif(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 112 | % compute controller gain 113 | obj.u = zeros(sim.nu,length(sim.t)); 114 | [obj.F,~,~] = dlqr(obj.A,obj.B,sim.Q,sim.R); 115 | obj.u(:,1) = -obj.F*obj.x(:,1); 116 | end 117 | end 118 | obj.trig_type = sim.TRIG_TYPE; 119 | obj.z = zeros(sim.nx,length(sim.t)); 120 | obj.z_curr = obj.z(:,1); 121 | obj.ID = ID; 122 | obj.comm_slot = zeros(1,length(sim.t)); 123 | obj.comm_trig = zeros(1,length(sim.t)); 124 | obj.comm_agents = zeros(1,sim.N); 125 | obj.sent_state = zeros(sim.nx,1); 126 | obj.all_sent_states = zeros(sim.nx,sim.N); 127 | if(strcmp(sim.TRIG_TYPE,'ST1')||strcmp(sim.TRIG_TYPE,'ST2')) 128 | obj.pot_comm_slot = zeros(1,length(sim.t)); 129 | end 130 | if(strcmp(sim.TRIG_TYPE,'PT') || strcmp(sim.TRIG_TYPE,'PPT')) 131 | obj.comm_prob = zeros(1,length(sim.t)); 132 | end 133 | end 134 | end 135 | 136 | 137 | % run agent dynamics for one timestep 138 | function process(obj,sim, net, k) 139 | % if(nargin < 5 && net.arbit_type == 2) 140 | % c = 0.75; 141 | % end 142 | 143 | if(strcmp(sim.PROB_TYPE,'ESTIMATION')) 144 | % update state 145 | obj.x(:,k) = obj.A*obj.x(:,k-1) + sim.eps(:,obj.ID); 146 | 147 | 148 | elseif(strcmp(sim.PROB_TYPE,'CONTROL')) 149 | if(strcmp(sim.SIM_TYPE,'AV')) 150 | % update state 151 | if(mod(obj.ID,sim.Nl(obj.lane)) == 1) 152 | obj.x(:,k) = obj.A*obj.x(:,k-1) + obj.B*obj.chi(:,k-1) + obj.Bff*sim.uref(obj.lane,k-1) + sim.eps(:,obj.ID); 153 | % using LQG 154 | % obj.x(:,k) = obj.A*obj.x(:,k-1) + obj.B*obj.chi(:,k-1) + (obj.B + obj.Bff)*sim.uref(obj.lane,k-1) + sim.eps(:,obj.ID); 155 | 156 | obj.se(k) = obj.x(1,k); 157 | obj.ad(k) = obj.x(4,k); 158 | 159 | % update vehicle pos,vel,acc,... 160 | obj.a(k) = (1-(sim.dt/sim.tau))*obj.a(k-1) + (sim.dt/sim.tau)*obj.ad(k-1); 161 | obj.v(k) = obj.v(k-1) + sim.dt*obj.a(k-1); 162 | obj.px(k) = obj.px(k-1) + sim.dt*obj.v(k-1); 163 | 164 | else % ID ~= 1 165 | obj.x(:,k) = obj.A*obj.x(:,k-1) + obj.B*obj.chi(:,k-1) + obj.Bff*obj.xhat(4,k-1,obj.ID-1) + sim.eps(:,obj.ID); 166 | % using LQG 167 | % obj.x(:,k) = obj.A*obj.x(:,k-1) + obj.B*obj.chi(:,k-1) + (obj.B + obj.Bff)*obj.xhat(4,k-1,obj.ID-1) + sim.eps(:,obj.ID); 168 | 169 | obj.se(k) = obj.x(1,k); 170 | obj.ad(k) = obj.x(4,k); 171 | 172 | % update vehicle pos,vel,acc,... 173 | obj.a(k) = (1-(sim.dt/sim.tau))*obj.a(k-1) + (sim.dt/sim.tau)*obj.ad(k-1); 174 | obj.v(k) = obj.v(k-1) + sim.dt*obj.a(k-1); 175 | obj.px(k) = obj.px(k-1) + sim.dt*obj.v(k-1); 176 | 177 | end 178 | 179 | % if(strcmp(sim.TRIG_TYPE,'ET')) 180 | % obj.tau(k) = obj.timeSinceLastTrig(k,sim.dt); 181 | % obj.eta(k) = obj.eta(k) + sim.dt*obj.psi(k-1); 182 | % end 183 | 184 | 185 | elseif(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 186 | 187 | obj.x(:,k) = obj.A*obj.x(:,k-1) + obj.B*obj.u(:,k-1) + sim.eps(:,obj.ID); 188 | 189 | end 190 | end 191 | 192 | % Communicate 193 | if(strcmp(sim.TRIG_TYPE,'ET')) 194 | obj.ET_comm(sim,k); 195 | 196 | elseif(strcmp(sim.TRIG_TYPE,'PT') || strcmp(sim.TRIG_TYPE,'PPT')) 197 | obj.PT_comm(sim,net,k); 198 | 199 | elseif(strcmp(sim.TRIG_TYPE,'ST1') || strcmp(sim.TRIG_TYPE,'ST2')) 200 | obj.ST1_comm(sim,sim.ST_M,k); 201 | 202 | end 203 | end 204 | 205 | 206 | % set agent predictions based on which agents have communicated their 207 | % states 208 | function predictors(obj,sim,k) 209 | 210 | % update all predicted states (deterministic model) 211 | if(strcmp(sim.PROB_TYPE,'ESTIMATION')) 212 | if(obj.comm_trig(k)) 213 | obj.xhat(:,k,obj.ID) = obj.x(:,k); 214 | for id = 1:size(obj.Ahat,3) 215 | if(id ~= obj.ID) 216 | obj.xhat(:,k,id) = obj.Ahat(:,:,id)*obj.xhat(:,k-1,id); 217 | end 218 | end 219 | else 220 | for id = 1:size(obj.Ahat,3) 221 | obj.xhat(:,k,id) = obj.Ahat(:,:,id)*obj.xhat(:,k-1,id); 222 | end 223 | end 224 | 225 | elseif(strcmp(sim.PROB_TYPE,'CONTROL')) 226 | if(strcmp(sim.SIM_TYPE,'AV')) 227 | 228 | for id = 1:size(obj.Ahat,3) 229 | if(mod(id,sim.Nl(obj.lane)) == 1) 230 | if(obj.comm_agents(id)==0) 231 | obj.xhat(:,k,id) = obj.Ahat(:,:,id)*obj.xhat(:,k-1,id) + obj.B*obj.chi(:,k-1) + obj.Bff*sim.uref(obj.lane,k-1); 232 | % obj.xhat(:,k,id) = obj.Ahat(:,:,id)*obj.xhat(:,k-1,id) + obj.B*obj.chi(:,k-1) + (obj.B + obj.Bff)*sim.uref(obj.lane,k-1); 233 | 234 | elseif(obj.comm_agents(id)==1) 235 | xcomm = obj.all_sent_states(:,id); 236 | obj.xhat(:,k,id) = xcomm; 237 | 238 | end 239 | 240 | else 241 | 242 | if(obj.comm_agents(id)==0) 243 | obj.xhat(:,k,id) = obj.Ahat(:,:,id)*obj.xhat(:,k-1,id) + obj.B*obj.chi(:,k-1) + obj.Bff*obj.xhat(4,k-1,id-1); 244 | % obj.xhat(:,k,id) = obj.Ahat(:,:,id)*obj.xhat(:,k-1,id) + obj.B*obj.chi(:,k-1) + (obj.B + obj.Bff)*obj.xhat(4,k-1,id-1); 245 | 246 | elseif(obj.comm_agents(id)==1) 247 | xcomm = obj.all_sent_states(:,id); 248 | obj.xhat(:,k,id) = xcomm; 249 | 250 | end 251 | end 252 | end 253 | 254 | 255 | elseif(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 256 | 257 | % update all predicted states (perfect model) 258 | if(obj.comm_trig(k)) 259 | obj.xhat(:,k,obj.ID) = obj.x(:,k); 260 | for id = 1:size(obj.Ahat,3) 261 | if(id ~= obj.ID) 262 | obj.xhat(:,k,id) = obj.Ahat(:,:,id)*obj.xhat(:,k-1,id) + obj.B*obj.u(:,k-1); 263 | end 264 | end 265 | else 266 | for id = 1:size(obj.Ahat,3) 267 | obj.xhat(:,k,id) = obj.Ahat(:,:,id)*obj.xhat(:,k-1,id) + obj.B*obj.u(:,k-1); 268 | end 269 | end 270 | 271 | end 272 | end 273 | end 274 | 275 | 276 | % calculate error Z of current agent 277 | function error(obj,k) 278 | obj.z(:,k) = obj.x(:,k) - obj.xhat(:,k,obj.ID); 279 | obj.z_curr = obj.z(:,k); 280 | 281 | end 282 | 283 | 284 | % update control input 285 | function controller(obj,sim,k) 286 | if(strcmp(sim.SIM_TYPE,'AV')) 287 | if(strcmp(obj.trig_type,'ET')) 288 | % ZOH method for ET 289 | % if(mod(obj.ID,sim.Nl(obj.lane)) == 1) 290 | % obj.chi(:,k) = obj.F(1:3)*obj.x(1:3,k) + sim.uref(obj.lane,k); 291 | % else 292 | % spacing_error = obj.ppreced - obj.px(k) - sim.L(obj.ID) - sim.r(obj.ID) - sim.h*obj.v(k); 293 | % 294 | % if(obj.comm_agents(obj.ID-1)==0) 295 | % obj.adtilde(k) = obj.adtilde(k-1); 296 | % elseif(obj.comm_agents(obj.ID-1)==1) 297 | % obj.adtilde(k) = obj.all_sent_states(4,obj.ID-1); 298 | % end 299 | % obj.chi(:,k) = obj.F(1:3)*[spacing_error;obj.x(2:3,k)] + obj.adtilde(k); 300 | % 301 | % end 302 | 303 | % model-prediction for ET 304 | if(mod(obj.ID,sim.Nl(obj.lane)) == 1) 305 | obj.chi(:,k) = obj.F(1:3)*obj.xhat(1:3,k,obj.ID) + sim.uref(obj.lane,k); 306 | else 307 | spacing_error = obj.ppreced - obj.px(k) - sim.L(obj.ID) - sim.r(obj.ID) - sim.h*obj.v(k); 308 | obj.chi(:,k) = obj.F(1:3)*[spacing_error;obj.xhat(2:3,k,obj.ID)] + obj.xhat(4,k,obj.ID-1); 309 | % obj.chi(:,k) = obj.F(1:3)*obj.xhat(1:3,k,obj.ID) + obj.xhat(4,k,obj.ID-1); 310 | 311 | end 312 | 313 | else 314 | 315 | if(mod(obj.ID,sim.Nl(obj.lane)) == 1) 316 | obj.chi(:,k) = obj.F(1:3)*obj.xhat(1:3,k,obj.ID) + sim.uref(obj.lane,k); 317 | else 318 | spacing_error = obj.ppreced - obj.px(k) - sim.L(obj.ID) - sim.r(obj.ID) - sim.h*obj.v(k); 319 | obj.chi(:,k) = obj.F(1:3)*[spacing_error;obj.xhat(2:3,k,obj.ID)] + obj.xhat(4,k,obj.ID-1); 320 | % obj.chi(:,k) = obj.F(1:3)*obj.xhat(1:3,k,obj.ID) + obj.xhat(4,k,obj.ID-1); 321 | 322 | end 323 | 324 | end 325 | 326 | elseif(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 327 | obj.u(:,k) = -obj.F*obj.xhat(:,k,obj.ID); 328 | 329 | end 330 | end 331 | 332 | 333 | % determine comm probability at time k+M taking horizon into account 334 | function pred_comm_prob(obj,sim,k) 335 | if(k <= length(sim.t) - sim.M) 336 | lookup = [sim.reset_prob; obj.H']; 337 | Pc = obj.comm_prob(k); 338 | numTerms = 2^sim.M; 339 | numSubTerms = sim.M+1; 340 | c = dec2bin(0:numTerms-1)-48; 341 | c = rot90(c,2); 342 | prob = 0; 343 | for n = 1:numTerms 344 | if(sum(c(n,:)) == sim.M) 345 | term = Pc*lookup(1,1)^sim.M; 346 | elseif(sum(c(n,:)) == 0) 347 | term = 1; 348 | for m = 1:sim.M 349 | if(m == sim.M) 350 | term = term*lookup(m); 351 | else 352 | term = term*(1-lookup(m)); 353 | end 354 | end 355 | term = term*(1-Pc); 356 | else 357 | 358 | term = 1; 359 | if(c(n,1)) 360 | term = term*Pc; 361 | else 362 | term = term*(1-Pc); 363 | end 364 | 365 | % c(s) determines 1-H or just H 366 | for s = 1:numSubTerms-1 367 | Z = 2; % set to error not zero row 368 | t = 0; 369 | for m = s:-1:1 370 | if(c(n,m)) 371 | Z = 1; % set to reset probability row 372 | t = s-m+1; 373 | break; 374 | else 375 | t = t+1; % increment timestep index 376 | end 377 | end 378 | 379 | if(s+1 <= numSubTerms-1) 380 | if(c(n,s+1)) 381 | term = term*lookup(Z,t); 382 | else 383 | term = term*(1-lookup(Z,t)); 384 | end 385 | else 386 | term = term*lookup(Z,t); 387 | end 388 | end 389 | 390 | end 391 | 392 | prob = prob + term; 393 | end 394 | 395 | obj.comm_prob(1,k+sim.M) = prob; 396 | obj.comm_prob_mstep = prob; 397 | end 398 | end 399 | 400 | 401 | % ARBITRATOR: Decides communication at current time 402 | function PT_comm(obj,sim,net,k,c) 403 | if(nargin < 5 && net.arbit_type == 2) 404 | c = 0.75; 405 | end 406 | 407 | obj.sent_state = zeros(sim.nx,1); 408 | 409 | % comm if assigned slot 410 | if(net.arbit_type == 0) 411 | if(obj.comm_slot(k)) 412 | % obj.xhat(:,k,obj.ID) = obj.x(:,k); 413 | obj.comm_trig(k) = obj.comm_slot(k); 414 | obj.sent_state = obj.x(:,k); 415 | end 416 | 417 | % comm if assigned slot and error > delta 418 | elseif(net.arbit_type == 1) 419 | if(obj.comm_slot(k)) 420 | % if error really is above threshold, communicate 421 | if(norm(obj.z(:,k-1)) >= sim.delta) 422 | obj.comm_trig(k) = 1; 423 | obj.sent_state = obj.x(:,k); 424 | end 425 | end 426 | 427 | % comm if assigned slot and error > c*delta 428 | elseif(net.arbit_type == 2) 429 | if(c > 1) 430 | error('Input a constant factor in (0,1)'); 431 | end 432 | if(obj.comm_slot(k)) 433 | if(norm(obj.z(:,k-1)) >= c*sim.delta) 434 | obj.comm_trig(k) = 1; 435 | obj.sent_state = obj.x(:,k); 436 | end 437 | end 438 | end 439 | 440 | obj.comm_trig_curr = obj.comm_trig(k); 441 | 442 | if(strcmp(sim.SIM_TYPE,'AV')) 443 | obj.psent = obj.px(k); 444 | end 445 | end 446 | 447 | function schedule_slot(obj,sim,k) 448 | % set slot assignment M step ahead 449 | if(k <= length(sim.t) - sim.M) 450 | obj.comm_slot(k+sim.M) = obj.comm_slot_mstep; 451 | end 452 | end 453 | 454 | 455 | % EVENT-TRIGGERED COMMUNICATION [no prediction] 456 | function ET_comm(obj,sim,k) 457 | obj.sent_state = zeros(sim.nx,1); 458 | 459 | % communicate at every timestep 460 | % if(obj.comm_slot(k)) 461 | % obj.comm_trig(k) = 1; 462 | % obj.sent_state = obj.x(:,k-1); 463 | % obj.comm_trig_curr = obj.comm_trig(k); 464 | % end 465 | 466 | if(obj.comm_slot(k)) 467 | if(norm(obj.z(:,k-1)) >= sim.delta) 468 | obj.comm_trig(k) = 1; 469 | obj.sent_state = obj.x(:,k); 470 | end 471 | end 472 | obj.comm_trig_curr = obj.comm_trig(k); 473 | 474 | if(strcmp(sim.SIM_TYPE,'AV')) 475 | obj.psent = obj.px(k); 476 | end 477 | end 478 | 479 | % function ET_Heemels(obj,sim,k) 480 | % obj.sent_state = zeros(sim.nx,1); 481 | % 482 | % if(obj.comm_slot(k)) 483 | % if(obj.eta(k-1) < 0) 484 | % obj.comm_trig(k) = 1; 485 | % obj.sent_state = obj.x(:,k); 486 | % end 487 | % end 488 | % obj.comm_trig_curr = obj.comm_trig(k); 489 | % 490 | % obj.psent = obj.px(k); 491 | % end 492 | 493 | 494 | % SELF-TRIGGERED COMMUNICATION 495 | function ST1_comm(obj,sim,ST_M,k) 496 | 497 | obj.sent_state = zeros(sim.nx,1); 498 | 499 | % communicate if slot assigned 500 | if(obj.comm_slot(k) == 1) 501 | % arbitration 502 | obj.xhat(:,k,obj.ID) = obj.x(:,k); 503 | obj.comm_trig(k) = obj.comm_slot(k); 504 | obj.sent_state = obj.x(:,k); 505 | 506 | % reset agent to ST 507 | if(~strcmp(obj.trig_type,'ST1')) 508 | obj.trig_type = 'ST1'; 509 | obj.M = ST_M; 510 | obj.comm_slot(k+1:k+obj.M-1) = 0; 511 | end 512 | 513 | % compute new M 514 | if(size(obj.A,3) ~= 1) % time-varying agent model 515 | end 516 | 517 | elseif(obj.comm_slot(k) == -1) 518 | obj.trig_type = 'PT'; 519 | obj.M = sim.M; 520 | end 521 | 522 | obj.comm_trig_curr = obj.comm_trig(k); 523 | end 524 | 525 | function ST2_comm(obj,t,M,k) 526 | 527 | obj.sent_state = zeros(sim.nx,1); 528 | 529 | % communicate if slot assigned 530 | if(obj.comm_slot(k)) 531 | % arbitration 532 | obj.xhat(:,k,obj.ID) = obj.x(:,k); 533 | obj.comm_trig(k) = obj.comm_slot(k); 534 | obj.sent_state = obj.x(:,k); 535 | 536 | % reset agent to ST 537 | if(~strcmp(obj.trig_type,'ST2')) 538 | obj.trig_type = 'ST2'; 539 | obj.M = M; 540 | obj.comm_slot(k+1:k+obj.M-1) = 0; 541 | end 542 | 543 | % compute new M 544 | if(size(obj.A,3) ~= 1) % time-varying agent model 545 | end 546 | 547 | % set next potential comm slot 548 | if(k+obj.M <= length(t)) 549 | obj.pot_comm_slot(k+obj.M) = 1; 550 | end 551 | end 552 | 553 | obj.comm_trig_curr = obj.comm_trig(k); 554 | end 555 | 556 | 557 | % Return number of errors above threshold 558 | function count = check_error(obj,sim) 559 | norm_z = obj.get_estError_norm(sim.nx); 560 | 561 | count = 0; 562 | for k = 1:length(sim.t) 563 | if(norm_z(k) >= sim.delta) 564 | 565 | if(k + sim.M <= length(sim.t)) 566 | if(norm_z(k+sim.M) >= sim.delta) 567 | count = count + 1; 568 | end 569 | end 570 | % consider all M prediction points 571 | % for m = sim.M:-1:1 572 | % if(norm_z(k+m) >= sim.delta) 573 | % 574 | % end 575 | % end 576 | end 577 | end 578 | % number of points above threshold 579 | % count = numel(norm_z(norm_z >= delta)); 580 | end 581 | 582 | 583 | % get error norm of agent for over entire simulation 584 | function znorm = get_estError_norm(obj,nx) 585 | if(nx > 1) 586 | znorm = sqrt(sum(obj.z.*obj.z)); 587 | else 588 | znorm = abs(obj.z); 589 | end 590 | end 591 | 592 | % returns mean estimation error norm of a simulation for this agent 593 | function zbari = mean_estError_norm(obj,nx) 594 | norm_z = obj.get_estError_norm(nx); 595 | zbari = mean(norm_z); 596 | end 597 | 598 | 599 | % get error norm of agent for over entire simulation 600 | function enorm = get_ctrlError_norm(obj,sim) 601 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 602 | xd = zeros(sim.nx,length(sim.t)); 603 | obj.e = obj.x - xd; 604 | 605 | if(sim.nx > 1) 606 | enorm = sqrt(sum(obj.e.*obj.e)); 607 | else 608 | enorm = abs(obj.e); 609 | end 610 | 611 | elseif(strcmp(sim.SIM_TYPE,'AV')) 612 | if(mod(obj.ID,sim.Nl(obj.lane)) == 1) 613 | obj.e = abs(obj.v - sim.vref(obj.lane,:)); 614 | enorm = obj.e; 615 | else 616 | obj.e = [obj.v;obj.d]-[sim.vref(obj.lane,:);obj.dr]; 617 | enorm = sqrt(sum(obj.e.*obj.e)); 618 | end 619 | end 620 | end 621 | 622 | % returns mean control error norm of a simulation for this agent 623 | function ebari = mean_ctrlError_norm(obj,sim) 624 | norm_e = obj.get_ctrlError_norm(sim); 625 | ebari = mean(norm_e); 626 | end 627 | 628 | 629 | function lane = getLane(~,ypos,lwidth) 630 | lane = 1 + (ypos - lwidth/2)/lwidth; 631 | lane = int8(lane); 632 | end 633 | 634 | function lastTrig = timeSinceLastTrig(obj,k,dt) 635 | commTimes = find(obj.comm_trig == 1); 636 | if(isempty(commTimes)) 637 | lastTrig = k*dt; 638 | else 639 | lastComm = commTimes(end); 640 | lastTrig = (k-lastComm)*dt; 641 | end 642 | end 643 | 644 | % Plot the state trajectories and the error of the agent over time 645 | function plotStates(obj,nx,t,delta) 646 | figure; 647 | for i = 1:nx 648 | subplot(nx+1,1,i); 649 | hold on; grid on; box on; 650 | plot(t,obj.x(i,:)); 651 | plot(t,obj.xhat(i,:,obj.ID)); 652 | xlabel('Time [s]'); 653 | ylabel(['x',num2str(i)]); 654 | if(i == 1) 655 | title(['Agent ',num2str(obj.ID)]); 656 | legend('Process','Prediction'); 657 | end 658 | end 659 | subplot(nx+1,1,nx+1); 660 | hold on; grid on; box on; 661 | norm_z = obj.get_estError_norm(nx); 662 | plot(t,norm_z); 663 | plot(t,delta*ones(1,length(t)),'r'); 664 | xlabel('Time [s]'); 665 | ylabel('||Z(t)||_{2}'); 666 | end 667 | 668 | function plotEstError(obj,nx,t,delta) 669 | figure; 670 | subplot(2,1,1) 671 | hold on; grid on; box on; 672 | norm_z = obj.get_estError_norm(nx); 673 | plot(t,norm_z); 674 | plot(t,delta*ones(1,length(t)),'r'); 675 | title(['Agent ',num2str(obj.ID)]); 676 | xlabel('Time [s]'); 677 | ylabel('||Z(t)||_{2}'); 678 | legend('State Error','Error Bound'); 679 | subplot(2,1,2); 680 | hold on; grid on; box on; 681 | stairs(t,0.5*obj.comm_slot(1:length(t)),'g'); 682 | stairs(t,obj.comm_trig,'k'); 683 | legend('Comm Slot','Comm Trigger'); 684 | xlabel('Time [s]'); 685 | ylim([0 1.5]); 686 | end 687 | 688 | function plotCtrlError(obj,sim) 689 | figure; 690 | subplot(2,1,1) 691 | hold on; grid on; box on; 692 | norm_e = obj.get_ctrlError_norm(sim); 693 | plot(sim.t,norm_e); 694 | title(['Agent ',num2str(obj.ID)]); 695 | xlabel('Time [s]'); 696 | ylabel('||e(t)||_{2}'); 697 | legend('Control Error'); 698 | subplot(2,1,2); 699 | hold on; grid on; box on; 700 | stairs(sim.t,0.5*obj.comm_slot(1:length(sim.t)),'g'); 701 | stairs(sim.t,obj.comm_trig,'k'); 702 | legend('Comm Slot','Comm Trigger'); 703 | xlabel('Time [s]'); 704 | ylim([0 1.5]); 705 | end 706 | 707 | end 708 | end -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_1M_4D_26z0_unstable.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_1M_4D_26z0_unstable.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_2M_4D_26z0_unstable.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_2M_4D_26z0_unstable.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_3M_4D_26z0_unstable.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_3M_4D_26z0_unstable.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_4M_4D_26z0_unstable.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_4M_4D_26z0_unstable.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_5M_4D_26z0_unstable.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/Mstep_lookup_AV_MC_STO_5M_4D_26z0_unstable.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/init_av_params.m: -------------------------------------------------------------------------------- 1 | %% 2 | % 3 | %-------------------------------------------------------------------------- 4 | 5 | fprintf('Initializing vehicle parameters...'); 6 | 7 | 8 | %-------------------------------------------------------------------------- 9 | % Mass & Length parameters 10 | %-------------------------------------------------------------------------- 11 | 12 | m = 1300; % vehicle mass [kg] 13 | l = 4; % vehicle length [m] 14 | g = 9.81; % gravity acceleration [m/s^2] 15 | lf = 1.1; % Distance of front wheel and CG [m] 16 | lr = 1.6; % Distance of front wheel and CG [m] 17 | h = 0.5; % height of vehicle CG [m] 18 | Jz = 2000; % Inertia around cg [kgm^2] 19 | 20 | %-------------------------------------------------------------------------- 21 | % Aerodynamic parameters 22 | %-------------------------------------------------------------------------- 23 | Cd = 0.3; % drag coefficient [] 24 | rho = 1.225; % air density [kg/m^3] 25 | A = 1.6 + 0.00056*(m - 765); % frontal area [m^2] 26 | Vw = 0; % headwind speed [m/s^2] 27 | 28 | %-------------------------------------------------------------------------- 29 | % Engine parameters 30 | %-------------------------------------------------------------------------- 31 | % tau_thr_max = [1,2,3,4,5,6]; % for each gear 32 | tau_thr_max = 260; % [Nm] 33 | 34 | %-------------------------------------------------------------------------- 35 | % Braking parameters 36 | %-------------------------------------------------------------------------- 37 | Cbrk = .1; % braking constant? 38 | 39 | %-------------------------------------------------------------------------- 40 | % Transmission parameters 41 | %-------------------------------------------------------------------------- 42 | xg = [3.643 2.080 1.361 1.024 0.830 0.686]; % gear ratio 43 | xd = 4.105; % differential ratio 44 | nt = 0.7; % transmission efficiency 45 | 46 | %-------------------------------------------------------------------------- 47 | % Tire parameters 48 | %-------------------------------------------------------------------------- 49 | Rw = 0.4; % wheel radius [m] 50 | mw = 9.5; % wheel mass [kg] 51 | Jw = mw*Rw^2; % 2-wheel inertia [kgm^2] 52 | 53 | tau_bs = 0.2; % brake pressure lag [s] 54 | Kb = 10; % pressure/torque conversion [Nm/bar] 55 | Kc = 1; % pressure gain 56 | 57 | B = 10; % Pacejka model coefficients 58 | C = 1.9; 59 | D = 1; 60 | E = 0.97; 61 | 62 | % Cs_f = ; % tire slip coefficient [N/deg] 63 | Ca_f = 100000*(pi/180); % front tire sideslip coefficient [N/deg] 64 | Ca_r = 120000*(pi/180); % rear tire sideslip coefficient [N/deg] 65 | Cr = 0.015; % rolling resistance coefficient 66 | 67 | 68 | %-------------------------------------------------------------------------- 69 | % Road parameters 70 | %-------------------------------------------------------------------------- 71 | theta = 0; % road inclination 72 | lanes = 3; % number of lanes 73 | lwidth = 3.7; % lane width [m] 74 | 75 | ylc = zeros(1,lanes); % lane center positions [m] 76 | for i = 1:lanes 77 | ylc(i) = lwidth/2 + (i-1)*lwidth; 78 | end 79 | 80 | 81 | fprintf('done\n'); -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/reset_lookup_AV_STO_1M_4D.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/reset_lookup_AV_STO_1M_4D.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/reset_lookup_AV_STO_2M_4D.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/reset_lookup_AV_STO_2M_4D.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/reset_lookup_AV_STO_3M_4D.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/reset_lookup_AV_STO_3M_4D.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/reset_lookup_AV_STO_4M_4D.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/reset_lookup_AV_STO_4M_4D.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/reset_lookup_AV_STO_5M_4D.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/josem93/resource_constrained_predictive_triggering/36ceb88f6533fdb4c8a659fc2d9c0f3fc6416bc0/source_code/Autonomous Vehicles/reset_lookup_AV_STO_5M_4D.mat -------------------------------------------------------------------------------- /source_code/Autonomous Vehicles/setLeadVelocity.m: -------------------------------------------------------------------------------- 1 | function vref = setLeadVelocity(tvec,dt) 2 | 3 | vref = zeros(1,length(tvec)); 4 | 5 | highwaySpeed = 100/3.6; % [m/s] 6 | trafficSpeed = 15/3.6; % [m/s] 7 | Zero2Hundred = 10; % [s] 8 | Hundred2Zero = 5; % [s] 9 | 10 | % acceleration to highway speed (1-11 seconds) 11 | t = tvec(101:1101); 12 | m_fAccel = highwaySpeed/Zero2Hundred; 13 | vref(101:1101) = m_fAccel*t - m_fAccel; 14 | 15 | % constant highway speed (11-31 sec) 16 | vref(1101:3100) = highwaySpeed; 17 | 18 | % basic decelerateion then acceleration back to highway speed 19 | % deceleration (31-41 sec) 20 | m = (0.75-1)*highwaySpeed/(41-31); 21 | t = tvec(3101:4100); 22 | vref(3101:4100) = m*t + (highwaySpeed-31*m); 23 | % acceleration (41-51 sec) 24 | b = (0.75*highwaySpeed) - 41*m_fAccel; 25 | t_end = (highwaySpeed - b)/m_fAccel; 26 | t = tvec(4101:t_end/dt); 27 | vref(4101:t_end/dt) = m_fAccel*t + b; 28 | 29 | % constant highway speed (38.5-60 sec) 30 | vref(t_end/dt:6000) = highwaySpeed; 31 | 32 | 33 | % Oscilatory velocity change (60-120 sec) [TODO] 34 | vref(6001:7000) = highwaySpeed; 35 | 36 | tend = 368; 37 | t = 0:dt:tend; 38 | rad2Hz = 0.1591549; 39 | method = 'linear'; 40 | phi = -90; 41 | fs1 = 0.06*rad2Hz; fe1 = 1.13*rad2Hz; 42 | fs2 = 1.13*rad2Hz; fe2 = 2.26*rad2Hz; 43 | fs3 = 2.26*rad2Hz; fe3 = 3.14*rad2Hz; 44 | fs4 = 2.26*rad2Hz; fe4 = 3.14*rad2Hz; 45 | 46 | y{1} = chirp(t,fs1,tend,fe1,method,phi); 47 | y{2} = chirp(t,fs2,tend,fe2,method,phi); 48 | y{3} = chirp(t,fs3,tend,fe3,method,phi); 49 | % y{4} = chirp(t,fs4,tend,fe4,method,phi); 50 | 51 | a = y{1}+y{2}+y{3}; 52 | v = cumtrapz(t,a); 53 | 54 | offset = highwaySpeed - v(12501); 55 | vref(6001:10362) = v(12501:16862)+offset; 56 | 57 | % figure; 58 | % for i = 1:3 59 | % subplot(3,1,i); 60 | % plot(t,y{i}); 61 | % xlim([125 175]); 62 | % end 63 | % figure; 64 | % hold on; grid on; box on; 65 | % plot(t,v); 66 | % ylim([9 19]); 67 | % xlim([125 175]); 68 | 69 | vref(10363:12000) = highwaySpeed; 70 | 71 | % %% 72 | % % traffic scenario speed back and forth (x3) 73 | % % sudden braking 74 | % m_fDecel = -highwaySpeed/Hundred2Zero; 75 | % m_hDecel = -highwaySpeed/(2*Hundred2Zero); 76 | % m = -0.5*highwaySpeed/(125-120); 77 | % t = tvec(12001:12500); 78 | % vref(12001:12500) = m*t + (highwaySpeed-m*120); 79 | % 80 | % vref(12501:13500) = highwaySpeed/2; 81 | % 82 | % m = -0.5*highwaySpeed/(140-135); 83 | % t = tvec(13501:14000); 84 | % vref(13501:14000) = m*t - 140*m; 85 | % 86 | % % constant zero speed (94-104 sec) 87 | % vref(14001:15500) = zeros(1,length(14001:15500)); 88 | % 89 | % % Phase 1 90 | % m_tAccel = trafficSpeed/(1.5*Zero2Hundred); 91 | % b_accel = -m_tAccel*155; 92 | % t_end = (trafficSpeed-b_accel)/m_tAccel; 93 | % t = tvec(15501:t_end/dt); 94 | % vref(15501:t_end/dt) = m_tAccel*t + b_accel; 95 | % 96 | % vref(t_end/dt:t_end/dt+200) = trafficSpeed; 97 | % 98 | % t_curr = t_end/dt+200; 99 | % 100 | % b_decel = trafficSpeed - t_curr*dt*m_hDecel; 101 | % t_end = -b_decel/m_hDecel; 102 | % t = tvec(t_curr+1:t_end/dt); 103 | % vref(t_curr+1:t_end/dt) = m_hDecel*t + b_decel; 104 | % 105 | % vref(t_end/dt:t_end/dt+500) = 0; 106 | % 107 | % t_curr = t_end/dt+500; 108 | % 109 | % % Phase 2 110 | % b_accel = -m_tAccel*t_curr*dt; 111 | % t_end = ((trafficSpeed/2)-b_accel)/m_tAccel; 112 | % t = tvec(t_curr+1:t_end/dt); 113 | % vref(t_curr+1:t_end/dt) = m_tAccel*t + b_accel; 114 | % 115 | % t_curr = t_end/dt; 116 | % 117 | % b_decel = (trafficSpeed/2) - t_curr*dt*m_hDecel; 118 | % t_end = -b_decel/m_hDecel; 119 | % t = tvec(t_curr+1:t_end/dt); 120 | % vref(t_curr+1:t_end/dt) = m_hDecel*t + b_decel; 121 | % 122 | % vref(t_end/dt:t_end/dt+500) = 0; 123 | % 124 | % t_curr = t_end/dt+500; 125 | % 126 | % % Phase 3 127 | % b_accel = -m_tAccel*t_curr*dt; 128 | % t_end = (trafficSpeed-b_accel)/m_tAccel; 129 | % t = tvec(t_curr+1:t_end/dt+1); 130 | % vref(t_curr+1:t_end/dt+1) = m_tAccel*t + b_accel; 131 | % 132 | % t_curr = t_end/dt; 133 | % 134 | % b_decel = trafficSpeed - t_curr*dt*m_hDecel; 135 | % t_end = ((trafficSpeed/2)-b_decel)/m_hDecel; 136 | % t = tvec(t_curr+1:t_end/dt); 137 | % vref(t_curr+1:t_end/dt) = m_hDecel*t + b_decel; 138 | % vref(t_end/dt:t_end/dt+200) = trafficSpeed/2; 139 | % 140 | % t_curr = t_end/dt+200; 141 | % 142 | % b_decel = trafficSpeed/2 - t_curr*dt*m_hDecel; 143 | % t_end = -b_decel/m_hDecel; 144 | % t = tvec(t_curr+1:t_end/dt); 145 | % vref(t_curr+1:t_end/dt) = m_hDecel*t + b_decel; 146 | % 147 | % vref(t_end/dt:t_end/dt+1000) = 0; 148 | % 149 | % t_curr = t_end/dt+1000; 150 | % 151 | % % Phase 2 152 | % b_accel = -m_tAccel*t_curr*dt; 153 | % t_end = ((trafficSpeed/2)-b_accel)/m_tAccel; 154 | % t = tvec(t_curr+1:t_end/dt); 155 | % vref(t_curr+1:t_end/dt) = m_tAccel*t + b_accel; 156 | % 157 | % t_curr = t_end/dt; 158 | % 159 | % b_decel = (trafficSpeed/2) - t_curr*dt*m_hDecel; 160 | % t_end = -b_decel/m_hDecel; 161 | % t = tvec(t_curr+1:t_end/dt); 162 | % vref(t_curr+1:t_end/dt) = m_hDecel*t + b_decel; 163 | % 164 | % vref(t_end/dt:t_end/dt+500) = 0; 165 | % 166 | % t_curr = t_end/dt+500; 167 | % 168 | % % Phase 3 169 | % b_accel = -m_tAccel*t_curr*dt; 170 | % t_end = (trafficSpeed-b_accel)/m_tAccel; 171 | % t = tvec(t_curr+1:t_end/dt); 172 | % vref(t_curr+1:t_end/dt) = m_tAccel*t + b_accel; 173 | % 174 | % t_curr = t_end/dt; 175 | % 176 | % b_decel = trafficSpeed - t_curr*dt*m_hDecel; 177 | % t_end = ((trafficSpeed/2)-b_decel)/m_hDecel; 178 | % t = tvec(t_curr+1:t_end/dt); 179 | % vref(t_curr+1:t_end/dt) = m_hDecel*t + b_decel; 180 | % vref(t_end/dt:t_end/dt+300) = trafficSpeed/2; 181 | % 182 | % t_curr = t_end/dt+300; 183 | % 184 | % b_decel = trafficSpeed/2 - t_curr*dt*m_hDecel; 185 | % t_end = -b_decel/m_hDecel; 186 | % t = tvec(t_curr+1:t_end/dt); 187 | % vref(t_curr+1:t_end/dt) = m_hDecel*t + b_decel; 188 | % 189 | % vref(t_end/dt:t_end/dt+200) = 0; 190 | % 191 | % t_curr = t_end/dt+200; 192 | % 193 | % % Phase 1 194 | % b_accel = -m_tAccel*t_curr*dt; 195 | % t_end = (trafficSpeed-b_accel)/m_tAccel; 196 | % t = tvec(t_curr+1:t_end/dt); 197 | % vref(t_curr+1:t_end/dt) = m_tAccel*t + b_accel; 198 | % 199 | % vref(t_end/dt:t_end/dt+300) = trafficSpeed; 200 | % 201 | % t_curr = t_end/dt+300; 202 | % 203 | % b_decel = trafficSpeed - t_curr*dt*m_hDecel; 204 | % t_end = -b_decel/m_hDecel; 205 | % t = tvec(t_curr+1:t_end/dt); 206 | % vref(t_curr+1:t_end/dt) = m_hDecel*t + b_decel; 207 | % 208 | % vref(t_end/dt:t_end/dt+2000) = 0; 209 | % 210 | % t_curr = t_end/dt+2000; 211 | % 212 | % 213 | % % Stop & Go 214 | % t = tvec(t_curr+1:t_curr+Zero2Hundred/dt); 215 | % vref(t_curr+1:t_curr+Zero2Hundred/dt) = m_fAccel*t - m_fAccel*t_curr*dt; 216 | % 217 | % t_curr = t_curr+Zero2Hundred/dt; 218 | % 219 | % vref(t_curr+1:t_curr+2000) = highwaySpeed; 220 | % 221 | % t_curr = t_curr+2000; 222 | % 223 | % t = tvec(t_curr+1:t_curr+Hundred2Zero/dt); 224 | % vref(t_curr+1:t_curr+Hundred2Zero/dt) = m_fDecel*t - (t_curr+(Hundred2Zero/dt))*dt*m_fDecel; 225 | % % filler just because of unknown error 226 | % % vref(16385) = m_fDecel*163.85 - (t_curr+(Hundred2Zero/dt))*dt*m_fDecel; 227 | % t_curr = t_curr+Hundred2Zero/dt; 228 | % 229 | % vref(t_curr+1:t_curr+2000) = 0; 230 | % 231 | % t_curr = t_curr+2000; 232 | % 233 | % t = tvec(t_curr+1:t_curr+Zero2Hundred/dt); 234 | % vref(t_curr+1:t_curr+Zero2Hundred/dt) = m_fAccel*t - m_fAccel*t_curr*dt; 235 | % 236 | % t_curr = t_curr+Zero2Hundred/dt; 237 | % 238 | % vref(t_curr+1:t_curr+2000) = highwaySpeed; 239 | % 240 | % t_curr = t_curr+2000; 241 | % 242 | % m_Decel = -highwaySpeed/(5*Hundred2Zero); 243 | % t = tvec(t_curr+1:t_curr+5*Hundred2Zero/dt); 244 | % vref(t_curr+1:t_curr+5*Hundred2Zero/dt) = m_Decel*t - (t_curr+(5*Hundred2Zero/dt))*dt*m_Decel; 245 | % 246 | % t_curr = t_curr+5*Hundred2Zero/dt; 247 | % 248 | % vref(t_curr+1:t_curr+2000) = 0; 249 | 250 | % plot(tvec,vref); 251 | 252 | -------------------------------------------------------------------------------- /source_code/CP_compare_ETvPT_real_experiment.m: -------------------------------------------------------------------------------- 1 | %% Comparison script between ET and PT real experiments 2 | clear; 3 | clc; 4 | 5 | cutTime = 5; % [sec] 6 | N = 10; % # of agents 7 | dt = 0.01; % time step size 8 | T = 30; % simulation time 9 | t = 0:dt:T; % time vector 10 | nx = 4; 11 | SYNC = 1; 12 | P_CONSTRAINT = 0; 13 | CAST_FLAG = 1; 14 | DIST_FLAG = 0; % {0 = disturbance off, 1 = disturbances on} 15 | REF_FLAG = 0; % {0 = reference off, 1 = reference on} 16 | % if(CAST_FLAG && P_CONSTRAINT) 17 | % trig_type = {'PText','ET1','ET2'}; 18 | % else 19 | % trig_type = {'PT','ET1','ET2'}; 20 | % end 21 | trig_type = {'PT','PText','ET1','ET2'}; 22 | Kset = [3,4,6,8,10]; 23 | c = 0.5; 24 | cs = num2str(c); 25 | dot_id = find(cs=='.'); 26 | cs = cs(dot_id+1:end); 27 | 28 | thisPath = pwd; 29 | if(SYNC) 30 | addpath([thisPath, '/sync_data_430']); 31 | else 32 | if(DIST_FLAG == 0 && REF_FLAG == 0) 33 | addpath([thisPath, '/data_refOFF_distOFF']); 34 | elseif(DIST_FLAG == 1 && REF_FLAG == 0) 35 | addpath([thisPath, '/data_refOFF_distON']); 36 | elseif(DIST_FLAG == 0 && REF_FLAG == 1) 37 | addpath([thisPath, '/data_refON_distOFF']); 38 | else 39 | error('No data!'); 40 | end 41 | end 42 | 43 | Zbar = zeros(length(trig_type),length(Kset)); 44 | Zvar = zeros(length(trig_type),length(Kset)); 45 | Ebar = zeros(length(trig_type),length(Kset)); 46 | Evar = zeros(length(trig_type),length(Kset)); 47 | mean_util = zeros(length(trig_type),length(Kset)); 48 | 49 | m = zeros(1,3); 50 | for tr = 1:length(trig_type) 51 | for k = 1:length(Kset) 52 | for n = 1:3 % number of trials 53 | if(tr == 1 || tr == 2) 54 | filename_agents = ['AGENT_DATA_',trig_type{tr},'_K',num2str(Kset(k)),'_c0',cs,'_',num2str(n),'.mat']; 55 | load(filename_agents); 56 | if(n == 1) 57 | filename_net = ['NET_DATA_',trig_type{tr},'_K',num2str(Kset(k)),'_c0',cs,'.mat']; 58 | load(filename_net); 59 | end 60 | else 61 | filename_agents = ['AGENT_DATA_',trig_type{tr},'_K',num2str(Kset(k)),'_',num2str(n),'.mat']; 62 | load(filename_agents); 63 | end 64 | 65 | d = (size(data,1)-1)/N; % # of data vectors 66 | AGENTS = cell(N,1); 67 | for ID = 1:N 68 | if(SYNC) 69 | temp = find(data(38,:)>0); 70 | % start_idx = temp(1); 71 | start_idx = 3001; % take last 30s 72 | tvec = t(start_idx:end); 73 | else 74 | start_idx = 1; 75 | if(size(AGENTS{ID}.x(1,:),2) == length(t)) 76 | tvec = t; 77 | else 78 | tvec = t(1:end-1); 79 | end 80 | end 81 | idx = d*(ID-1)+2:d*ID+1; 82 | agent_data = data(idx,:); 83 | AGENTS{ID}.x = agent_data(1:4,start_idx:end); 84 | AGENTS{ID}.xhat = agent_data(5:8,start_idx:end); 85 | AGENTS{ID}.xref = agent_data(9,start_idx:end); 86 | AGENTS{ID}.u_stable = agent_data(10,start_idx:end); 87 | AGENTS{ID}.u_noisy = agent_data(11,start_idx:end); 88 | AGENTS{ID}.z = agent_data(12:15,start_idx:end); 89 | AGENTS{ID}.P = agent_data(16,start_idx:end); 90 | AGENTS{ID}.comm_slot = agent_data(17,start_idx:end); 91 | AGENTS{ID}.comm_trig = agent_data(18,start_idx:end); 92 | AGENTS{ID}.comm_trig_util = agent_data(18,:); 93 | 94 | % cap state when pendulum goes unstable 95 | % if(ID == 1 && k == 1) 96 | % ids = find(AGENTS{ID}.x(2,:) >= 3); 97 | % if(~isempty(ids)) 98 | % AGENTS{ID}.x(:,ids(1)+1:length(tvec)+1) = [AGENTS{ID}.x(1:3,ids(1)).*ones(3,length(tvec)+1-ids(1)); THETA_MAX*ones(1,length(tvec)+1-ids(1))]; 99 | % end 100 | % end 101 | end 102 | 103 | if(tr == 1 || tr == 2) 104 | if(n == 1) 105 | Np = net_data(2,start_idx:end); 106 | end 107 | elseif(tr == 4) 108 | Np = N; 109 | else 110 | Np = zeros(1,length(t)); 111 | end 112 | 113 | % Bandwidth 114 | if(tr == 2) 115 | P_CONSTRAINT = 1; 116 | else 117 | P_CONSTRAINT = 0; 118 | end 119 | if(P_CONSTRAINT) 120 | b = (Kset(k)*4*nx + (4+2)*N)/dt; 121 | else 122 | b = (Kset(k)*4*nx + 4*N)/dt; 123 | end 124 | % b = (Kset(k)*4*nx + 4*N)/dt; 125 | % capacity per time step [bytes] 126 | capacity = dt*b; 127 | u(n) = mean(util(AGENTS,Np,start_idx,nx,capacity,tr,CAST_FLAG)); 128 | 129 | [zbar,zvar,ebar,evar] = men(AGENTS,SYNC); 130 | m(n) = ebar; 131 | end 132 | 133 | % [zbar,zvar,ebar,evar] = men(AGENTS,SYNC); 134 | % Zbar(tr,k) = zbar; Zvar(tr,k) = zvar; 135 | % Ebar(tr,k) = ebar; Evar(tr,k) = evar; 136 | Ebar(tr,k) = mean(m); 137 | mean_util(tr,k) = mean(u); 138 | clear AGENTS; 139 | fclose('all'); 140 | end 141 | end 142 | 143 | % Plot 144 | pos = [0 0 813 645]; 145 | plot_settings(18,1.5); 146 | % estimation error 147 | % figure('Position',pos); 148 | % hold on;grid on; box on; 149 | % for tr = 1:length(trig_type) 150 | % plot(Kset, Zbar(tr,:),'-s'); 151 | % % errorbar(K,Zbar(tr,:),Zvar(tr,:),'-s'); 152 | % end 153 | % xlabel('Number of communication slots [K]'); 154 | % ylabel('Mean Estimation Error Norm [$\bar{Z}$]'); 155 | % title(['Experiment Trigger Comparison, N = ',num2str(N)]); 156 | % l = legend(trig_type); 157 | % l.Orientation = 'horizontal'; 158 | % l.Location = 'northeast'; 159 | % xlim([0 Kset(end)+2]); 160 | 161 | % control error 162 | fig = figure('Position',pos); 163 | color = [0 0 0]; 164 | hold on;grid on; box on; 165 | if(SYNC) 166 | xlim([0 Kset(end)+1]); 167 | xlabel('Number of communication slots $K$'); 168 | title(['Experiment Trigger Comparison, N = ',num2str(N),', c = ',num2str(c)]); 169 | for tr = 1:length(trig_type) 170 | plot(Kset,Ebar(tr,:),'-s'); 171 | end 172 | l = legend(trig_type); 173 | 174 | else 175 | for tr = 1:length(trig_type) 176 | % plot(Kset, Ebar(tr,:),'-s'); 177 | errorbar(Kset,Ebar(tr,:),Evar(tr,:),'-s'); 178 | end 179 | xlabel('Number of communication slots $K$'); 180 | ylabel('Mean Control Error Norm $\bar{E}$'); 181 | title(['Experiment Trigger Comparison, N = ',num2str(N)]); 182 | l = legend(trig_type); 183 | l.Orientation = 'horizontal'; 184 | l.Location = 'northeast'; 185 | xlim([2 Kset(end)+1]); 186 | end 187 | 188 | % network utilization 189 | figure('Position',pos); 190 | hold on;grid on; box on; 191 | for tr = 1:length(trig_type) 192 | plot(Kset,mean_util(tr,:),'-s'); 193 | end 194 | xlabel('Number of communication slots $K$'); 195 | ylabel('Mean Network Utilization [$\%$]'); 196 | title(['Trigger comparison, N = ',num2str(N)]); 197 | l = legend(trig_type); 198 | xlim([2 Kset(end)+1]); 199 | 200 | 201 | if(SYNC) 202 | rmpath([thisPath, '/sync_data']); 203 | else 204 | if(DIST_FLAG == 0 && REF_FLAG == 0) 205 | rmpath([thisPath, '/data_refOFF_distOFF']); 206 | elseif(DIST_FLAG == 1 && REF_FLAG == 0) 207 | rmpath([thisPath, '/data_refOFF_distON']); 208 | elseif(DIST_FLAG == 0 && REF_FLAG == 1) 209 | rmpath([thisPath, '/data_refON_distOFF']); 210 | else 211 | error('No data!'); 212 | end 213 | end 214 | 215 | 216 | % computes the mean estimation and control error norms (+ variances) 217 | function [mean_est_error_norm,var_est_error_norm,mean_ctrl_error_norm,var_ctrl_error_norm] = men(AGENTS,sync) 218 | if(sync) 219 | sim_znorms = []; sim_enorms = []; 220 | ref = AGENTS{3}.x(1,:); 221 | for i = 1:length(AGENTS) 222 | if(i ~= 3) 223 | sim_enorms = [sim_enorms; norm(AGENTS{i}.x(1,:) - ref)]; 224 | end 225 | est_norm = sqrt(sum(AGENTS{i}.z.*AGENTS{i}.z)); 226 | sim_znorms = [sim_znorms; est_norm]; 227 | end 228 | mean_z_agents = mean(sim_znorms,2); 229 | mean_est_error_norm = mean(mean_z_agents); 230 | var_est_error_norm = mean(std(sim_znorms,0,2)); 231 | mean_ctrl_error_norm = mean(sim_enorms); 232 | var_ctrl_error_norm = mean(std(sim_enorms,0,2)); 233 | 234 | else 235 | sim_znorms = []; sim_enorms = []; 236 | for i = 1:length(AGENTS) 237 | if(i ~= 1) 238 | ctrl_norm = sqrt(sum(AGENTS{i}.x.*AGENTS{i}.x)); 239 | sim_enorms = [sim_enorms; ctrl_norm]; 240 | est_norm = sqrt(sum(AGENTS{i}.z.*AGENTS{i}.z)); 241 | sim_znorms = [sim_znorms; est_norm]; 242 | end 243 | end 244 | mean_z_agents = [mean(sqrt(sum(AGENTS{1}.z.*AGENTS{1}.z))); mean(sim_znorms,2)]; 245 | mean_est_error_norm = mean(mean_z_agents); 246 | var_est_error_norm = mean([std(mean(sqrt(sum(AGENTS{1}.z.*AGENTS{1}.z)))); std(sim_znorms,0,2)]); 247 | mean_e_agents = [mean(sqrt(sum(AGENTS{1}.x.*AGENTS{1}.x))); mean(sim_enorms,2)]; 248 | mean_ctrl_error_norm = mean(mean_e_agents); 249 | var_ctrl_error_norm = mean(std(sim_enorms,0,2)); 250 | end 251 | end 252 | 253 | % calculate network utilization 254 | function U = util(AGENTS,Np,idx,nx,capacity,tr,cast) 255 | commDecisions = []; 256 | for i = 1:length(AGENTS) 257 | commDecisions = [commDecisions; AGENTS{i}.comm_trig_util(idx:end)]; 258 | end 259 | Nc = sum(commDecisions,1); 260 | if(tr == 1 || tr == 2) %PT, PText, ET2 261 | if(cast) 262 | U = 100*(Np + 4*nx*Nc)/capacity; 263 | else 264 | U = 100*(4*Np + 4*nx*Nc)/capacity; 265 | end 266 | else 267 | U = 100*(4*Np + 4*nx*Nc)/capacity; 268 | end 269 | 270 | % if(P_CONSTRAINT||TRIG_TYPE == 4) 271 | % net_util = 100*((4+2)*Np + 4*nx*Nc)/capacity; 272 | % else 273 | % end 274 | end -------------------------------------------------------------------------------- /source_code/Mstep_probability.m: -------------------------------------------------------------------------------- 1 | function H = Mstep_probability(sim,trig_type,M,z) 2 | % Function to calculate Pr(tau > M*dt | Z(0) = z) of all agents by using 3 | % the precomputed lookup tables 4 | % 5 | % Inputs: 6 | % sim structure containing the simulation parameters 7 | % trig_type trigger type [string] 8 | % M prediction horizon 9 | % z error of all agents 10 | % 11 | % Outputs: 12 | % H exit probability of all agents 13 | % 14 | %-------------------------------------------------------------------------- 15 | 16 | if(M == 0) % ET 17 | znorm = sqrt(sum(z.*z)); 18 | H = znorm >= sim.delta; 19 | else 20 | if(strcmp(trig_type,'PT') || strcmp(trig_type,'PPT')) 21 | H = zeros(M,size(z,2)); 22 | 23 | for m = 1:M 24 | for i = 1:size(z,2) 25 | if(norm(z(:,i)) >= sim.delta) 26 | H(:,i) = ones(M,1); 27 | else 28 | z0 = num2cell(z(:,i)); 29 | H(m,i) = interpn(sim.Z0_grid{:},sim.lookup_prob{m},z0{:}); 30 | end 31 | end 32 | end 33 | 34 | elseif(strcmp(trig_type,'ST') || strcmp(trig_type,'ST2')) 35 | 36 | H = zeros(1,size(z,2)); 37 | for i = 1:size(z,2) 38 | if(norm(z(:,i)) >= sim.delta) 39 | H(i) = 1; 40 | else 41 | z0 = num2cell(z(:,i)); 42 | H(i) = interpn(sim.Z0_grid{:},sim.sched_delay_lookup,z0{:}); 43 | end 44 | end 45 | 46 | end 47 | end -------------------------------------------------------------------------------- /source_code/Mstep_probability_setup.m: -------------------------------------------------------------------------------- 1 | function sim = Mstep_probability_setup(varargin) 2 | % Function to either load or compute the lookup tables for the desired 3 | % prediction horizon 4 | % 5 | % Inputs: 6 | % sim structure containing the simulation parameters 7 | % sched_delay scheduling delay used in ST design 8 | 9 | % Outputs: 10 | % sim update simulation parameter structure with the lookup 11 | % tables 12 | % 13 | %-------------------------------------------------------------------------- 14 | 15 | sim = varargin{1}; 16 | 17 | s = 'stable'; 18 | lambda = abs(eig(sim.A)); 19 | for i = 1:length(lambda) 20 | if(lambda(i) >= 1) 21 | s = 'unstable'; 22 | break; 23 | end 24 | end 25 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 26 | if(sum(diag(sim.A)) ~= 0.95*sim.nx && strcmp(s,'stable')) 27 | s = [s, '_rand']; 28 | end 29 | if(sum(diag(sim.A)) ~= 1.1*sim.nx && strcmp(s,'unstable')) 30 | s = [s, '_rand']; 31 | end 32 | end 33 | 34 | if(strcmp(sim.FET_TYPE,'MC')) 35 | %%------------------------------------------------------------------------- 36 | % Monte Carlo Simulation Solution 37 | %-------------------------------------------------------------------------- 38 | sim.numSim = 1e4; % number of MC simulation 39 | sim.MC_type = 'STO'; % Monte Carlo Type {'STO','ERR'} 40 | 41 | 42 | if(strcmp(sim.TRIG_TYPE,'PT') || strcmp(sim.TRIG_TYPE,'PPT')) 43 | temp = sim.M; 44 | for m = 1:sim.M 45 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 46 | folder = 'Stochastic Processes'; 47 | 48 | % check if lookup table already exists and if not run a Monte Carlo 49 | % simulation to generate it 50 | filename1 = ['Mstep_lookup_',sim.FET_TYPE,'_',sim.MC_type,'_',num2str(m),'M_',... 51 | num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 52 | filename2 = ['reset_lookup_',sim.MC_type,'_',... 53 | num2str(m),'M_',num2str(sim.nx),'D_',s,'.mat']; 54 | 55 | elseif(strcmp(sim.SIM_TYPE,'AV')) 56 | folder = 'Autonomous Vehicles'; 57 | filename1 = ['Mstep_lookup_AV_',sim.FET_TYPE,'_',sim.MC_type,'_',num2str(m),'M_',... 58 | num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 59 | filename2 = ['reset_lookup_AV_',sim.MC_type,'_',... 60 | num2str(m),'M_',num2str(sim.nx),'D','.mat']; 61 | 62 | elseif(strcmp(sim.SIM_TYPE,'CP')) 63 | folder = 'Cart-Pole'; 64 | filename1 = ['Mstep_lookup_CP_',sim.FET_TYPE,'_',sim.MC_type,'_',num2str(m),'M_',... 65 | num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 66 | filename2 = ['reset_lookup_CP_',sim.MC_type,'_',... 67 | num2str(m),'M_',num2str(sim.nx),'D','.mat']; 68 | 69 | end 70 | 71 | f1 = exist(filename1,'file'); 72 | f2 = exist(filename2,'file'); 73 | if(f1 == 0 || f2 == 0) 74 | sim.M = m; 75 | run_Monte_Carlo(sim,filename1,filename2,folder); 76 | end 77 | 78 | P = load(filename1); 79 | prob{m} = P.prob; 80 | if(m == sim.M) 81 | load(filename2); 82 | end 83 | 84 | % if nargin < 2 85 | % sim.lookup_prob = prob; 86 | % sim.reset_prob = reset_prob; 87 | % else 88 | % mc = varargin{2}; 89 | % if(strcmp(mc,'STO')) 90 | % sim.lookup_prob_sto = prob; 91 | % elseif(strcmp(mc,'ERR')) 92 | % sim.lookup_prob_err = prob; 93 | % end 94 | % end 95 | end 96 | sim.M = temp; 97 | sim.lookup_prob = prob; 98 | sim.reset_prob = reset_prob; 99 | clear prob reset_prob P; 100 | 101 | elseif(strcmp(sim.TRIG_TYPE,'ST')||strcmp(sim.TRIG_TYPE,'ST2')) 102 | 103 | % lookup table to determine M 104 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 105 | folder = 'Stochastic Processes'; 106 | filename1 = ['ST_NUM_lookup_1D.mat']; 107 | % filename1 = [sim.TRIG_TYPE,'_',sim.FET_TYPE,'_lookup_',num2str(sim.nx),'D.mat']; 108 | elseif(strcmp(sim.SIM_TYPE,'AV')) 109 | folder = 'Autonomous Vehicles'; 110 | filename1 = [sim.TRIG_TYPE,'_AV_lookup',num2str(sim.nx),'D.mat']; 111 | end 112 | 113 | 114 | f = exist(filename1,'file'); 115 | if(f == 0) 116 | error('No file for ST M lookup'); 117 | % getExitProbPDE(sim,filename1,folder); 118 | end 119 | 120 | load(filename1); 121 | sim.ST_M_lookup = prob; 122 | clear prob; 123 | 124 | % lookup table for scheduling delay 125 | temp = sim.M; 126 | sim.M = varargin{2}; 127 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 128 | filename2 = ['Mstep_lookup_',sim.FET_TYPE,'_',sim.MC_type,'_',num2str(sim.M),... 129 | 'M_',num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 130 | elseif(strcmp(sim.SIM_TYPE,'AV')) 131 | filename2 = ['Mstep_lookup_AV_',sim.FET_TYPE,'_',num2str(sim.M),'M_',... 132 | num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 133 | end 134 | 135 | f = exist(filename2,'file'); 136 | if(f == 0) 137 | sim.TRIG_TYPE = 'PT'; 138 | getExitProbPDE(sim,filename2,folder); 139 | sim.TRIG_TYPE = 'ST2'; 140 | end 141 | sim.M = temp; 142 | 143 | load(filename2); 144 | sim.sched_delay_lookup = prob; 145 | clear prob; 146 | 147 | 148 | % lookup table for when agent changes to PT 149 | prob = []; 150 | for m = 1:sim.M 151 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 152 | filename3 = ['Mstep_lookup_',sim.FET_TYPE,'_',sim.MC_type,'_',num2str(m),... 153 | 'M_',num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 154 | elseif(strcmp(sim.SIM_TYPE,'AV')) 155 | filename3 = ['Mstep_lookup_AV_',sim.FET_TYPE,'_',num2str(m),'M_',... 156 | num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 157 | end 158 | 159 | f = exist(filename3,'file'); 160 | if(f == 0) 161 | sim.TRIG_TYPE = 'PT'; 162 | getExitProbPDE(sim,filename3,folder); 163 | sim.TRIG_TYPE = 'ST2'; 164 | end 165 | 166 | % lookup tables for m-step probs 167 | P = load(filename3); 168 | prob{m} = P.prob; 169 | end 170 | 171 | sim.lookup_prob = prob; 172 | clear prob P; 173 | end 174 | 175 | 176 | elseif(strcmp(sim.FET_TYPE,'NUM')) 177 | %%------------------------------------------------------------------------- 178 | % Numerical Solution 179 | %-------------------------------------------------------------------------- 180 | if(strcmp(sim.TRIG_TYPE,'PT') || strcmp(sim.TRIG_TYPE,'PPT')) 181 | sim.MC_type = 'STO'; % Monte Carlo Type {'STO','ERR'} 182 | temp = sim.M; 183 | for m = 1:sim.M 184 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 185 | folder = 'Stochastic Processes'; 186 | filename1 = ['Mstep_lookup_',sim.FET_TYPE,'_',num2str(sim.M),... 187 | 'M_',num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 188 | filename2 = ['reset_lookup_',sim.MC_type,'_',... 189 | num2str(sim.M),'M_',num2str(sim.nx),'D','.mat']; 190 | 191 | elseif(strcmp(sim.SIM_TYPE,'AV')) 192 | folder = 'Autonomous Vehicles'; 193 | filename1 = ['Mstep_lookup_AV_',sim.FET_TYPE,'_',num2str(m),'M_',... 194 | num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 195 | filename2 = ['reset_lookup_AV_',sim.MC_type,'_',... 196 | num2str(m),'M_',num2str(sim.nx),'D','.mat']; 197 | 198 | % filename1 = ['Mstep_lookup_AV_',sim.FET_TYPE,'_',num2str(sim.M),'M_',... 199 | % num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 200 | % filename2 = ['reset_lookup_AV_',sim.MC_type,'_',... 201 | % num2str(sim.M),'M_',num2str(sim.nx),'D','.mat']; 202 | end 203 | 204 | f1 = exist(filename1,'file'); 205 | f2 = exist(filename2,'file'); 206 | if(f1 == 0 || f2 == 0) 207 | sim.M = m; 208 | getExitProbPDE(sim,filename1,filename2,folder); 209 | end 210 | 211 | P = load(filename1); 212 | prob{m} = P.prob; 213 | R = load(filename2); 214 | reset(m) = R.reset_prob; 215 | 216 | end 217 | sim.M = temp; 218 | sim.lookup_prob = prob; 219 | sim.reset_prob = reset; 220 | clear prob reset_prob; 221 | 222 | elseif(strcmp(sim.TRIG_TYPE,'ST')||strcmp(sim.TRIG_TYPE,'ST2')) 223 | % lookup table to determine M 224 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 225 | folder = 'Stochastic Processes'; 226 | filename1 = [sim.TRIG_TYPE,'_',sim.FET_TYPE,'_lookup_',num2str(sim.nx),'D.mat']; 227 | elseif(strcmp(sim.SIM_TYPE,'AV')) 228 | folder = 'Autonomous Vehicles'; 229 | filename1 = [sim.TRIG_TYPE,'_',sim.FET_TYPE,'_AV_lookup',num2str(sim.nx),'D.mat']; 230 | end 231 | 232 | f = exist(filename1,'file'); 233 | if(f == 0) 234 | getExitProbPDE(sim,filename1,folder); 235 | end 236 | 237 | load(filename1); 238 | sim.ST_M_lookup = prob; 239 | clear prob; 240 | 241 | 242 | % lookup table for scheduling delay 243 | temp = sim.M; 244 | sim.M = varargin{2}; 245 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 246 | filename2 = ['Mstep_lookup_',sim.FET_TYPE,'_',num2str(sim.M),... 247 | 'M_',num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 248 | elseif(strcmp(sim.SIM_TYPE,'AV')) 249 | filename2 = ['Mstep_lookup_AV_',sim.FET_TYPE,'_',num2str(sim.M),'M_',... 250 | num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 251 | end 252 | 253 | f = exist(filename2,'file'); 254 | if(f == 0) 255 | sim.TRIG_TYPE = 'PT'; 256 | getExitProbPDE(sim,filename2,folder); 257 | sim.TRIG_TYPE = 'ST2'; 258 | end 259 | sim.M = temp; 260 | 261 | load(filename2); 262 | sim.sched_delay_lookup = prob; 263 | clear prob; 264 | 265 | 266 | % lookup table for when agent changes to PT 267 | for m = 1:M 268 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 269 | filename3 = ['Mstep_lookup_',sim.FET_TYPE,'_',num2str(m),... 270 | 'M_',num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 271 | elseif(strcmp(sim.SIM_TYPE,'AV')) 272 | filename3 = ['Mstep_lookup_AV_',sim.FET_TYPE,'_',num2str(m),'M_',... 273 | num2str(sim.nx),'D_',num2str(length(sim.Z0_set)),'z0_',s,'.mat']; 274 | end 275 | 276 | f = exist(filename3,'file'); 277 | if(f == 0) 278 | sim.TRIG_TYPE = 'PT'; 279 | getExitProbPDE(sim,filename3,folder); 280 | sim.TRIG_TYPE = 'ST2'; 281 | end 282 | 283 | % lookup table for m-step prob 284 | P = load(filename3); 285 | prob{m} = P.prob; 286 | end 287 | sim.lookup_prob = prob; 288 | clear prob P; 289 | end 290 | end 291 | -------------------------------------------------------------------------------- /source_code/Multi_Agent_Simulation.m: -------------------------------------------------------------------------------- 1 | % Script to run the Multi-agent simulation 2 | % 3 | % sim: structure containing the simulation parameters 4 | % net: structure containing the network parameters 5 | % 6 | %-------------------------------------------------------------------------- 7 | 8 | close all; 9 | clear; 10 | clc; 11 | 12 | seed = 100; 13 | rng(seed); 14 | 15 | %% Initialize simulation, PT, and Network parameters 16 | [sim,net] = params(); 17 | 18 | 19 | %% Setup lookup tables 20 | 21 | sim.dz0 = floor(200/(2*sim.nx)); 22 | sim.Z0_set = -sim.delta:2*sim.delta/(sim.dz0):sim.delta; 23 | 24 | % grids for interpolation 25 | sim.Z0_grid = cell(sim.nx,1); 26 | [sim.Z0_grid{:}] = ndgrid(sim.Z0_set); 27 | 28 | % Initalize lookup tablesWTB VS The Odd Couple: Movie Trivia Schmoedown 29 | if(strcmp(sim.TRIG_TYPE,'ST')||strcmp(sim.TRIG_TYPE,'ST2')) 30 | net.sched_delay = 1; 31 | sim = Mstep_probability_setup(sim,net.sched_delay); 32 | elseif(sim.M ~= 0) 33 | sim = Mstep_probability_setup(sim); 34 | end 35 | 36 | 37 | %% Network config 38 | net.util = zeros(1,length(sim.t)); % network utilization [%] 39 | 40 | % set network bandwidth [bytes/s for K slots and depending on the scenario] 41 | if(sim.P_CONSTRAINT) 42 | net.b = (net.K*4*sim.nx + (4+2)*sim.N)/sim.dt; 43 | else 44 | net.b = (net.K*4*sim.nx + 4*sim.N)/sim.dt; 45 | end 46 | 47 | net = network_setup(sim,net,sim.N); 48 | 49 | 50 | %% ET slot allocation method (ET1 in for loop) 51 | 52 | % if(strcmp(sim.ET_TYPE,'ET2')) 53 | % % ET2: random selection of agents (OLD) 54 | % idx = randperm(sim.N,net.K); 55 | % 56 | % elseif(strcmp(sim.ET_TYPE,'ET3')) 57 | % % ET3: leader agents 58 | % kpl = fix(net.K/sim.lanes); 59 | % rem = mod(net.K,sim.lanes); 60 | % extras = zeros(1,sim.lanes); 61 | % extraIDs = []; ids = []; 62 | % for n = 1:sim.lanes 63 | % extras(n) = sim.Nl(n) - kpl; 64 | % for i = 1:sim.Nl(n) 65 | % if(i > kpl) 66 | % ID = sum(sim.Nl(1:n-1)) + i; 67 | % extraIDs = [extraIDs ID]; 68 | % end 69 | % end 70 | % end 71 | % if(rem ~= 0) 72 | % if(sum(extras) > rem) 73 | % ids = extraIDs(randperm(numel(extraIDs),rem)); 74 | % end 75 | % end 76 | % end 77 | 78 | 79 | %% Initialize agents 80 | for ID = 1:sim.N 81 | AGENTS(ID) = Agent(sim,ID); 82 | 83 | if(strcmp(sim.TRIG_TYPE,'ET')) 84 | if(sim.N <= net.K) 85 | AGENTS(ID).comm_slot(:) = 1; 86 | end 87 | end 88 | 89 | % compute first ST M value and assign first slot 90 | if(strcmp(sim.TRIG_TYPE,'ST') || strcmp(sim.TRIG_TYPE,'ST2')) 91 | sim.ST_M = find(sim.ST_M_lookup > sim.ub,1); 92 | if(isempty(sim.M)) 93 | sim.ST_M = 30; 94 | end 95 | AGENTS(ID).M = sim.ST_M; 96 | 97 | if(strcmp(sim.TRIG_TYPE,'ST2')) 98 | AGENTS(ID).potcomm_slot(sim.ST_M+1) = 1; 99 | end 100 | end 101 | end 102 | 103 | 104 | %% Run simulation for k steps (T runtime) 105 | fprintf('Simulation in progress...'); 106 | reverseStr = ''; 107 | tSim = tic; 108 | totTies = 0; 109 | 110 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 111 | eps = sqrt(sim.var)*randn(sim.nx,sim.N,length(sim.t)); 112 | elseif(strcmp(sim.SIM_TYPE,'AV')) 113 | eps(1,:,:) = sqrt(sim.var2)*randn(1,sim.N,length(sim.t)); 114 | eps(2,:,:) = sqrt(sim.var2)*randn(1,sim.N,length(sim.t)); 115 | eps(3,:,:) = sqrt(sim.var2)*randn(1,sim.N,length(sim.t)); 116 | eps(4,:,:) = sqrt(sim.var3)*randn(1,sim.N,length(sim.t)); 117 | end 118 | 119 | for k = 2:length(sim.t) 120 | 121 | sim.eps = eps(:,:,k); 122 | 123 | % randomly assign slots for ET1 124 | if(strcmp(sim.TRIG_TYPE,'ET1')) 125 | if(sim.N > net.K) 126 | idx = randperm(sim.N,net.K); 127 | for ID = 1:sim.N 128 | if(sum(idx == ID) == 1) 129 | AGENTS(ID).comm_slot(k) = 1; 130 | end 131 | end 132 | end 133 | end 134 | 135 | % run process for all agents 136 | arrayfun( @(obj) obj.process(sim,net,k), AGENTS); 137 | 138 | if(strcmp(sim.SIM_TYPE,'AV')) 139 | psent = [AGENTS.psent]; 140 | 141 | for n = 1:sim.lanes 142 | for i = 1:sim.Nl(n) 143 | ID = sum(sim.Nl(1:n-1)) + i; 144 | if(mod(ID,sim.Nl(n)) ~= 1) 145 | AGENTS(ID).ppreced = psent(ID-1); 146 | end 147 | end 148 | end 149 | end 150 | 151 | % reset agent predictions given communications 152 | commDecisions = [AGENTS.comm_trig_curr]; 153 | for ID = 1:sim.N 154 | AGENTS(ID).comm_agents = commDecisions; 155 | end 156 | 157 | % create array for states of communicating agents; 158 | sentStates = [AGENTS.sent_state]; 159 | for ID = 1:sim.N 160 | AGENTS(ID).all_sent_states = sentStates; 161 | end 162 | 163 | % run predictor blocks for all agents 164 | arrayfun( @(obj) obj.predictors(sim,k), AGENTS); 165 | 166 | % run error for all agents 167 | arrayfun( @(obj) obj.error(k), AGENTS); 168 | 169 | if(strcmp(sim.PROB_TYPE,'CONTROL')) 170 | % run controller for all agents 171 | arrayfun( @(obj) obj.controller(sim,k), AGENTS); 172 | end 173 | 174 | 175 | if(strcmp(sim.TRIG_TYPE,'ET')) 176 | if(strcmp(sim.ET_TYPE,'ET1')) 177 | Np = 0; % no information sent to scheduler 178 | 179 | % Communicate 180 | arrayfun( @(obj) obj.ET_comm(sim,k), AGENTS); 181 | elseif(strcmp(sim.ET_TYPE,'ET2')) 182 | Np = sim.N; % all agents send error norm to scheduler 183 | 184 | % all agent errors 185 | errors = [AGENTS.z_curr]; 186 | 187 | % priority measure 188 | znorms = sqrt(sum(errors.*errors)); 189 | 190 | % send to Network manager 191 | slots = NetworkManager_ET(sim,net,znorms); 192 | args = num2cell(slots); 193 | [AGENTS.comm_slot_mstep] = deal(args{:}); 194 | 195 | % Schedule slots 196 | arrayfun( @(obj) obj.schedule_slot(sim,k), AGENTS); 197 | end 198 | 199 | elseif(strcmp(sim.TRIG_TYPE,'PT')) 200 | 201 | % all agent errors 202 | errors = [AGENTS.z_curr]; 203 | 204 | % get m-step probabilities for all agents 205 | H = Mstep_probability(sim,sim.TRIG_TYPE,sim.M,errors); 206 | for ID = 1:sim.N 207 | AGENTS(ID).H = H(:,ID); 208 | end 209 | 210 | % compute the communication probability at time k+M 211 | if(sim.M == 0) 212 | mstep_probs = H; 213 | else 214 | arrayfun( @(obj) obj.pred_comm_prob(sim,k), AGENTS); 215 | mstep_probs = [AGENTS.comm_prob_mstep]; 216 | end 217 | 218 | % send to Network Manager and assign slots 219 | if(sim.P_CONSTRAINT) 220 | mstep_send = []; ids = []; 221 | for ID = 1:sim.N 222 | if(mstep_probs(ID) > net.p/net.d) 223 | mstep_send = [mstep_send, mstep_probs(ID)]; 224 | ids = [ids ID]; 225 | end 226 | end 227 | Np = length(mstep_send); 228 | if(sim.CAST_FLAG) 229 | mstep_send = uint8(100*mstep_send); 230 | end 231 | if(Np == sim.N) 232 | [slots,ties] = NetworkManager(sim,net,mstep_send); 233 | else 234 | [slots,ties] = NetworkManager(sim,net,mstep_send,ids); 235 | end 236 | 237 | else 238 | Np = sim.N; 239 | if(sim.CAST_FLAG) 240 | mstep_send = uint8(100*mstep_probs); 241 | else 242 | mstep_send = mstep_probs; 243 | end 244 | [slots,ties] = NetworkManager(sim,net,mstep_send); 245 | end 246 | 247 | args = num2cell(slots); 248 | [AGENTS.comm_slot_mstep] = deal(args{:}); 249 | 250 | % Schedule slots 251 | arrayfun( @(obj) obj.schedule_slot(sim,k), AGENTS); 252 | 253 | 254 | elseif(strcmp(sim.TRIG_TYPE,'PPT')) 255 | if(mod(k,sim.PT_period) == 0) 256 | 257 | % all agent errors 258 | errors = [AGENTS.z_curr]; 259 | if(strcmp(sim.SIM_TYPE,'AV')) 260 | errors = errors(1,:); 261 | end 262 | 263 | % get m-step probabilities for all agents 264 | H = Mstep_probability(sim,sim.TRIG_TYPE,sim.M,errors); 265 | for ID = 1:sim.N 266 | AGENTS(ID).H = H(:,ID); 267 | end 268 | 269 | % compute the communication probability at time k+M 270 | arrayfun( @(obj) obj.pred_comm_prob(sim,k), AGENTS); 271 | mstep_probs = [AGENTS.comm_prob_mstep]; 272 | 273 | % send to Network Manager and assign slots 274 | if(sim.P_CONSTRAINT) 275 | mstep_send = []; ids = []; 276 | for ID = 1:sim.N 277 | if(mstep_probs(ID) > net.p/net.d) 278 | mstep_send = [mstep_send, mstep_probs(ID)]; 279 | ids = [ids ID]; 280 | end 281 | end 282 | Np = length(mstep_send); 283 | if(sim.CAST_FLAG) 284 | mstep_send = uint8(100*mstep_send); 285 | end 286 | if(Np == sim.N) 287 | [slots,ties] = NetworkManager(sim,net,mstep_send); 288 | else 289 | [slots,ties] = NetworkManager(sim,net,mstep_send,ids); 290 | end 291 | 292 | else 293 | Np = sim.N; 294 | if(sim.CAST_FLAG) 295 | mstep_send = uint8(100*mstep_probs); 296 | else 297 | mstep_send = mstep_probs; 298 | end 299 | [slots,ties] = NetworkManager(sim,net,mstep_send); 300 | end 301 | 302 | args = num2cell(slots); 303 | [AGENTS.comm_slot_mstep] = deal(args{:}); 304 | 305 | % Schedule slots 306 | arrayfun( @(obj) obj.schedule_slot(sim,k), AGENTS); 307 | 308 | else 309 | Np = 0; 310 | end 311 | 312 | elseif(strcmp(sim.TRIG_TYPE,'ST')) 313 | 314 | send = []; ids = []; 315 | for ID = 1:sim.N 316 | if(k <= length(sim.t)-AGENTS(ID).M) 317 | if(strcmp(AGENTS(ID).trig_type,'PT')) 318 | error = AGENTS(ID).z_curr; 319 | prob = Mstep_probability(sim,AGENTS(ID).trig_type,AGENTS(ID).M,error); 320 | send = [send, prob(end)]; 321 | ids = [ids, ID]; 322 | %ADD prediction horizon 323 | 324 | elseif(strcmp(AGENTS(ID).trig_type,'ST')) 325 | if(k == 2) 326 | send = [send AGENTS(ID).M]; 327 | ids = 1:sim.N; 328 | else 329 | if(AGENTS(ID).comm_trig(k)) 330 | send = [send AGENTS(ID).M]; 331 | ids = [ids ID]; 332 | end 333 | end 334 | end 335 | end 336 | end 337 | 338 | % send to Network Manager and assign slots 339 | if(~isempty(send)) 340 | Np = length(send); 341 | [slots,~] = NetworkManager(sim,net,send,ids); 342 | 343 | for m = 1:size(slots,2) 344 | for ID = 1:sim.N 345 | % if agent switches to PT 346 | if(strcmp(AGENTS(ID).trig_type,'PT')) 347 | if(m == sim.M) 348 | if(k + AGENTS(ID).M <= length(sim.t)) 349 | AGENTS(ID).comm_slot(k+AGENTS(ID).M) = slots(ID,m); 350 | end 351 | end 352 | elseif(strcmp(AGENTS(ID).trig_type,'ST')) 353 | % if it hasnt already been given a slot 354 | if(~AGENTS(ID).comm_slot(k+m)) 355 | AGENTS(ID).comm_slot(k+m) = slots(ID,m); 356 | end 357 | end 358 | end 359 | end 360 | else 361 | Np = 0; 362 | end 363 | % Communicate 364 | arrayfun( @(obj) obj.ST1_comm(sim,sim.ST_M,k), AGENTS); 365 | 366 | % OLD: not used anymore 367 | elseif(strcmp(sim.TRIG_TYPE,'ST2')) 368 | 369 | % get agent error that may communicate at next time step 370 | mstep_probs = []; 371 | ids = []; 372 | for ID = 1:sim.N 373 | if(k <= length(sim.t)-net.sched_delay) 374 | if(strcmp(AGENTS(ID).trig_type,'PT')) 375 | error = AGENTS(ID).z_curr; 376 | mstep_probs = [mstep_probs, Mstep_probability(sim,AGENTS(ID).trig_type,AGENTS(ID).M,error)]; 377 | ids = [ids, ID]; 378 | elseif(strcmp(AGENTS(ID).trig_type,'ST2')) 379 | if(AGENTS(ID).pot_comm_slot(k+net.sched_delay)) 380 | error = AGENTS(ID).z_curr; 381 | mstep_probs = [mstep_probs, Mstep_probability(sim,AGENTS(ID).trig_type,AGENTS(ID).M,error)]; 382 | ids = [ids, ID]; 383 | end 384 | end 385 | end 386 | end 387 | 388 | if(~isempty(mstep_probs)) 389 | % check if # of time steps need to be assigned is > than M 390 | if(ceil(sim.N/net.K) > sim.ST_M) 391 | i = 1; 392 | nA_sched = 0; 393 | NAS = []; 394 | while(true) 395 | nA_sched = 0; 396 | for ID = 1:sim.N 397 | if(AGENTS(ID).comm_slot(k+i)) 398 | nA_sched = nA_sched + 1; 399 | end 400 | end 401 | NAS = [NAS nA_sched]; 402 | if(nA_sched < net.K) 403 | break; 404 | end 405 | i = i+1; 406 | end 407 | else 408 | NAS = 0; 409 | end 410 | % send to Network Manager and assign slots 411 | Np = length(mstep_probs); 412 | [slots,~] = NetworkManager(sim,net,mstep_probs,ids,NAS(end)); 413 | if(k + size(slots,2) <= length(sim.t)) 414 | for m = 1:size(slots,2) 415 | for ID = 1:sim.N 416 | % if agent switches to PT 417 | if(slots(ID,1) == -1) 418 | AGENTS(ID).trig_type = 'PT'; 419 | AGENTS(ID).M = 2; 420 | if(k + AGENTS(ID).M <= length(sim.t)) 421 | AGENTS(ID).comm_slot(k+AGENTS(ID).M) = 0; 422 | end 423 | else 424 | if(strcmp(AGENTS(ID).trig_type,'PT')) 425 | if(m == 1) 426 | if(k + AGENTS(ID).M <= length(sim.t)) 427 | AGENTS(ID).comm_slot(k+AGENTS(ID).M) = slots(ID,1); 428 | end 429 | end 430 | elseif(strcmp(AGENTS(ID).trig_type,'ST2')) 431 | % if it hasnt already been given a slot 432 | if(~AGENTS(ID).comm_slot(k+net.sched_delay-1+length(NAS)-1+m)) 433 | AGENTS(ID).comm_slot(k+net.sched_delay-1+length(NAS)-1+m) = slots(ID,m); 434 | end 435 | end 436 | end 437 | end 438 | end 439 | end 440 | else 441 | Np = 0; 442 | end 443 | 444 | % Communicate 445 | arrayfun( @(obj) obj.ST_comm(sim.t,sim.ST_M,k), AGENTS); 446 | 447 | end 448 | 449 | % network utilization 450 | Nc = sum(commDecisions); 451 | if(sim.P_CONSTRAINT||strcmp(sim.TRIG_TYPE,'ST')||strcmp(sim.TRIG_TYPE,'ST2')) 452 | if(sim.CAST_FLAG) 453 | net.util(k) = 100*((1+2)*Np + 4*sim.nx*Nc)/net.capacity; 454 | else 455 | net.util(k) = 100*((4+2)*Np + 4*sim.nx*Nc)/net.capacity; 456 | end 457 | else 458 | if(sim.CAST_FLAG) 459 | net.util(k) = 100*(Np + 4*sim.nx*Nc)/net.capacity; 460 | else 461 | net.util(k) = 100*(4*Np + 4*sim.nx*Nc)/net.capacity; 462 | end 463 | end 464 | 465 | % total number of ties 466 | if(strcmp(sim.TRIG_TYPE,'PT')) 467 | totTies = totTies +ties; 468 | end 469 | 470 | % display percentage completed 471 | percentDone = floor(100*k/length(sim.t)); 472 | msg = sprintf('%i', percentDone); 473 | fprintf([reverseStr, msg]); 474 | fprintf('%%'); 475 | reverseStr = repmat(sprintf('\b'), 1, length(msg)+1); 476 | end 477 | sim.runtime = toc(tSim); 478 | fprintf([reverseStr, 'Done\n\n']); 479 | fprintf(['\t Simulation completed in ',num2str(sim.runtime),' seconds\n\n']); 480 | 481 | 482 | %% POSTPROCESSING 483 | 484 | disp(['Number of time steps w/ critical ties = ', num2str(totTies)]); 485 | disp(['Percentage = ', num2str(totTies/length(sim.t))]); 486 | 487 | % Check to see if any agent errors are above delta 488 | numAbove = 0; 489 | for i = 1:sim.N 490 | a(i) = AGENTS(i).check_error(sim); 491 | numAbove = numAbove + a(i); 492 | end 493 | if(numAbove == 0) 494 | disp('All agent estimation errors below threshold'); 495 | else 496 | disp('An agent estimation errors exceeds threshold'); 497 | end 498 | 499 | % Plot 500 | % pos = [759 302 813 645]; 501 | % plot_settings(18,1.5); 502 | 503 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 504 | if(strcmp(sim.PROB_TYPE,'ESTIMATION')) 505 | for ID = 1:10 506 | % AGENTS(i).plotStates(sim.nx,sim.t,sim.delta); 507 | AGENTS(ID).plotEstError(sim.nx,sim.t,sim.delta); 508 | end 509 | 510 | elseif(strcmp(sim.PROB_TYPE,'CONTROL')) 511 | for ID = 1:10 512 | AGENTS(i).plotStates(sim.nx,sim.t,sim.delta); 513 | % AGENTS(ID).plotCtrlError(sim); 514 | end 515 | end 516 | figure; 517 | stairs(sim.t,net.util); 518 | xlabel('Time [s]'); 519 | ylabel('Network Utilization [%]'); 520 | 521 | elseif(strcmp(sim.SIM_TYPE,'AV')) 522 | 523 | % compute inter-vehicle distance (d) 524 | for n = 1:sim.lanes 525 | for i = 1:sim.Nl(n) 526 | if(i == 1) 527 | for k = 2:length(sim.t) 528 | % distance to reference vehicle 529 | AGENTS(i).d(k) = sim.pref(k) - AGENTS(i).px(k) - sim.L(i); 530 | end 531 | else 532 | ID = sum(sim.Nl(1:n-1)) + i; 533 | for k = 2:length(sim.t) 534 | AGENTS(ID).d(k) = AGENTS(ID-1).px(k) - AGENTS(ID).px(k) - sim.L(ID); 535 | AGENTS(ID).dr(k) = sim.r(ID) + sim.h*AGENTS(ID).v(k); 536 | % AGENTS(ID).d(k) = AGENTS(ID).d(k-1) + sim.dt*(AGENTS(ID-1).v(k-1)-AGENTS(ID).v(k-1)); 537 | end 538 | end 539 | end 540 | end 541 | 542 | % Check for crash 543 | crash = 0; 544 | pos_all = [AGENTS.px]; 545 | pos_all = reshape(pos_all,length(sim.t),sim.N); 546 | pos_all = pos_all'; 547 | dp = abs(diff(pos_all)); 548 | for i = 1:sim.N 549 | if(i ~= 1) 550 | for j = 1:size(dp,2) 551 | if(dp(i-1,j) <= sim.L(i-1)) 552 | crash = 1; 553 | disp([num2str(i-1),',',num2str(j)]); 554 | disp('CRASH!!!'); 555 | break; 556 | end 557 | end 558 | end 559 | if(crash) 560 | break; 561 | end 562 | end 563 | 564 | % compute mean control error norm 565 | for ID = 1:sim.N 566 | ebar(ID) = AGENTS(ID).mean_ctrlError_norm(sim); 567 | end 568 | enorm = mean(ebar) 569 | 570 | % vehicle velocities 571 | for n = 1:sim.lanes 572 | figure; 573 | % figure('Position',pos); 574 | hold on;grid on;box on; 575 | title(['Lane ',num2str(n), ' - ',sim.TRIG_TYPE,' K = ',num2str(net.K),' Norm = ',num2str(enorm)]); 576 | for i = 1:sim.Nl(n)+1 577 | if(i == 1) 578 | plot(sim.t,sim.vref(n,:)); 579 | leg{i} = ['vref']; 580 | else 581 | ID = sum(sim.Nl(1:n-1)) + (i-1); 582 | plot(sim.t,AGENTS(ID).v); 583 | % plot(sim.t,AGENTS(ID).x(1,:)); 584 | % plot(sim.t,AGENTS(ID).xhat(1,:,2)); 585 | leg{i} = ['v',num2str(i-1)]; 586 | end 587 | end 588 | xlabel('Time [s]'); 589 | ylabel('Velocity [m/s]'); 590 | l = legend(leg); 591 | l.Location = 'best'; 592 | clear leg; 593 | end 594 | 595 | % vehicle position 596 | % for n = 1:sim.lanes 597 | % figure; 598 | % hold on;grid on;box on; 599 | % title(['Lane',num2str(n)]); 600 | % for i = 1:sim.Nl(n)+1 601 | % if(i == 1) 602 | % plot(sim.t,sim.pref(n,:)); 603 | % else 604 | % ID = sum(sim.Nl(1:n-1)) + (i-1); 605 | % plot(sim.t,AGENTS(ID).px); 606 | % end 607 | % end 608 | % end 609 | 610 | % inter-vehice distance 611 | for n = 1:sim.lanes 612 | figure; 613 | % figure('Position',pos); 614 | hold on;grid on;box on; 615 | title(['Lane ',num2str(n), ' - ',sim.TRIG_TYPE,' K = ',num2str(net.K),' Norm = ',num2str(enorm)]); 616 | plot(sim.t,AGENTS(2).dr,'g--'); 617 | leg{1} = ['desired']; 618 | for i = 1:sim.Nl(n) 619 | if(i ~= 1) 620 | ID = sum(sim.Nl(1:n-1)) + i; 621 | plot(sim.t,AGENTS(ID).d); 622 | leg{i} = ['v',num2str(i-1),'v',num2str(i)]; 623 | end 624 | end 625 | xlabel('Time [s]'); 626 | ylabel('Inter-vehicle Distance [m]'); 627 | l = legend(leg); 628 | l.Location = 'best'; 629 | clear leg; 630 | end 631 | 632 | % for ID = 1:sim.N 633 | % % AGENTS(i).plotStates(sim.nx,sim.t,sim.delta); 634 | % AGENTS(ID).plotEstError(sim.nx,sim.t,sim.delta); 635 | % end 636 | 637 | % figure; 638 | % stairs(sim.t,net.util); 639 | % xlabel('Time [s]'); 640 | % ylabel('Network Utilization [%]'); 641 | 642 | end 643 | 644 | -------------------------------------------------------------------------------- /source_code/NetworkManager.m: -------------------------------------------------------------------------------- 1 | function [slots,tie] = NetworkManager(varargin) 2 | % Scheduler: network slot assignment for time k+M 3 | % 4 | % sim structure containing the simulation parameters 5 | % net structure containing the network parameters 6 | % H Mstep comm. prob. of all agents 7 | % 8 | % Outputs: 9 | % slots slots allocation for all agents (binary {0,1}) 10 | % ties if a critical tie occurs at the current time step 11 | % (critical is if tie occurs at the Kth slot) 12 | %-------------------------------------------------------------------------- 13 | 14 | 15 | if(nargin < 5) 16 | if(nargin < 4) 17 | constraint = 0; 18 | if(nargin < 3) 19 | error('Not enough input arguments'); 20 | end 21 | else 22 | % PT w/ lower bound constraint 23 | constraint = 1; 24 | ids = varargin{4}; 25 | end 26 | else 27 | % ST 28 | ids = varargin{4}; 29 | NAS = varargin{5}; 30 | end 31 | 32 | sim = varargin{1}; 33 | net = varargin{2}; 34 | H = varargin{3}; 35 | trig_type = sim.TRIG_TYPE; 36 | tie = 0; 37 | 38 | 39 | if(strcmp(trig_type,'PT') || strcmp(trig_type,'PPT')) 40 | if(~constraint) 41 | %------------------------------------------------------------------ 42 | % METHOD 1 - PT or PPT (with no constraint on which probs sent): 43 | % K largest communication probabilities probabilities 44 | %------------------------------------------------------------------ 45 | if(net.sched_type == 0) 46 | if(sim.N <= net.K) 47 | slots = ones(1,sim.N); 48 | 49 | else 50 | slots = zeros(1,sim.N); 51 | 52 | % sort based on hitting probabilities 53 | [Hs,ID] = sort(H,'descend'); 54 | [C,~,IC] = unique(Hs,'stable'); 55 | 56 | % if ties occur, randomize 57 | if(length(Hs) ~= length(C)) 58 | if(Hs(net.K) == Hs(net.K+1)) 59 | n1 = numel(find(IC(1:net.K)==IC(net.K))); 60 | n2 = numel(find(IC==IC(net.K))); 61 | a = ID(net.K-n1+1:net.K+n2-n1); 62 | a = a(randperm(n2)); 63 | ID(net.K-n1+1:net.K+n2-n1) = a; 64 | tie = tie + 1; 65 | end 66 | end 67 | 68 | for i = 1:net.K 69 | slots(ID(i)) = 1; 70 | end 71 | end 72 | 73 | %------------------------------------------------------------------ 74 | % METHOD 2 - PT or PPT (with no constraint on which probs sent): 75 | % Assign slots to agents w/ prob > h 76 | %------------------------------------------------------------------ 77 | elseif(net.sched_type == 1) 78 | slots = zeros(1,sim.N); 79 | setI = find(H > net.h); 80 | 81 | % more agents have prob > h 82 | if(length(setI) > net.K) 83 | % sort based on hitting probabilities 84 | [Hs,ID] = sort(H,'descend'); 85 | [C,~,IC] = unique(Hs,'stable'); 86 | 87 | % if ties occur, randomize 88 | if(length(Hs) ~= length(C)) 89 | if(Hs(net.K) == Hs(net.K+1)) 90 | n1 = numel(find(IC(1:net.K)==IC(net.K))); 91 | n2 = numel(find(IC==IC(net.K))); 92 | a = ID(net.K-n1+1:net.K+n2-n1); 93 | a = a(randperm(n2)); 94 | ID(net.K-n1+1:net.K+n2-n1) = a; 95 | tie = tie + 1; 96 | end 97 | end 98 | for i = 1:net.K 99 | slots(ID(i)) = 1; 100 | end 101 | % random set of agents > h 102 | % b = sort(randsample(setI,network.K,0)); 103 | % slots(b) = 1; 104 | else 105 | slots = double((H > net.h)); 106 | end 107 | end 108 | 109 | else 110 | %------------------------------------------------------------------ 111 | % METHOD 1 - PT or PPT (with constraint prob > h/d): 112 | % K largest communication probabilities probabilities 113 | %------------------------------------------------------------------ 114 | if(net.sched_type == 0) 115 | slots = zeros(1,sim.N); 116 | if(length(ids) <= net.K) 117 | if(~isempty(ids)) 118 | slots(ids) = 1; 119 | end 120 | 121 | else 122 | % sort based on hitting probabilities 123 | [Hs,ID] = sort(H,'descend'); 124 | ids = ids(ID); 125 | [C,~,IC] = unique(Hs,'stable'); 126 | 127 | % if ties occur, randomize 128 | if(length(Hs) ~= length(C)) 129 | if(Hs(net.K) == Hs(net.K+1)) 130 | n1 = numel(find(IC(1:net.K)==IC(net.K))); 131 | n2 = numel(find(IC==IC(net.K))); 132 | a = ids(net.K-n1+1:net.K+n2-n1); 133 | a = a(randperm(n2)); 134 | ids(net.K-n1+1:net.K+n2-n1) = a; 135 | tie = tie + 1; 136 | % ties = length(H)-length(C); 137 | end 138 | end 139 | 140 | for i = 1:net.K 141 | slots(ids(i)) = 1; 142 | end 143 | end 144 | 145 | %------------------------------------------------------------------ 146 | % METHOD 2 - PT or PPT (with no constraint on which probs sent): 147 | % Assign slots to agents w/ prob > h 148 | %------------------------------------------------------------------ 149 | elseif(net.sched_type == 1) 150 | slots = zeros(1,sim.N); 151 | setI = find(H > net.h); 152 | 153 | % more agents have prob > h 154 | if(length(setI) > net.K) 155 | % sort based on hitting probabilities 156 | [Hs,ID] = sort(H,'descend'); 157 | ids = ids(ID); 158 | [C,~,IC] = unique(Hs,'stable'); 159 | 160 | % if ties occur, randomize 161 | if(length(Hs) ~= length(C)) 162 | if(Hs(net.K) == Hs(net.K+1)) 163 | n1 = numel(find(IC(1:net.K)==IC(net.K))); 164 | n2 = numel(find(IC==IC(net.K))); 165 | a = ids(net.K-n1+1:net.K+n2-n1); 166 | a = a(randperm(n2)); 167 | ids(net.K-n1+1:net.K+n2-n1) = a; 168 | tie = tie + 1; 169 | end 170 | end 171 | for i = 1:net.K 172 | slots(ids(i)) = 1; 173 | end 174 | 175 | else 176 | slots(ids(setI)) = 1; 177 | end 178 | end 179 | 180 | end 181 | 182 | 183 | elseif(strcmp(trig_type,'ST')) 184 | % in this case H represents the M values sent to NetMan 185 | % priority goes first to PT agents 186 | 187 | %split into probs and Ms 188 | Hm = []; Hp = []; IDm = []; IDp = []; 189 | for n = 1:length(H) 190 | if(isreal(H(n)) && rem(H(n),1)==0 && H(n)~=1) 191 | Hm = [Hm H(n)]; 192 | IDm = [IDm ids(n)]; 193 | else 194 | Hp = [Hp H(n)]; 195 | IDp = [IDp ids(n)]; 196 | end 197 | end 198 | 199 | maxM = max(Hm); 200 | slots = zeros(sim.N,maxM); 201 | 202 | % assign PT agents 203 | if(net.sched_type == 0) 204 | if(length(Hp) > net.K) 205 | [Hs,id] = sort(Hp,'descend'); 206 | IDp = IDp(id); 207 | [C,~,IC] = unique(Hs,'stable'); 208 | 209 | % if ties occur, randomize 210 | if(length(Hs) ~= length(C)) 211 | if(Hs(net.K) == Hs(net.K+1)) 212 | n1 = numel(find(IC(1:net.K)==IC(net.K))); 213 | n2 = numel(find(IC==IC(net.K))); 214 | a = IDp(net.K-n1+1:net.K+n2-n1); 215 | a = a(randperm(n2)); 216 | IDp(net.K-n1+1:net.K+n2-n1) = a; 217 | tie = tie + 1; 218 | end 219 | end 220 | for k = 1:net.K 221 | slots(IDp(k),sim.M) = 1; 222 | end 223 | else 224 | slots(IDp,sim.M) = 1; 225 | end 226 | 227 | elseif(net.sched_type == 1) 228 | setI = find(Hp > net.h); 229 | 230 | % more agents have prob > h 231 | if(length(setI) > net.K) 232 | [Hs,id] = sort(Hp,'descend'); 233 | IDp = IDp(id); 234 | [C,~,IC] = unique(Hs,'stable'); 235 | 236 | % if ties occur, randomize 237 | if(length(Hs) ~= length(C)) 238 | if(Hs(net.K) == Hs(net.K+1)) 239 | n1 = numel(find(IC(1:net.K)==IC(net.K))); 240 | n2 = numel(find(IC==IC(net.K))); 241 | a = IDp(net.K-n1+1:net.K+n2-n1); 242 | a = a(randperm(n2)); 243 | IDp(net.K-n1+1:net.K+n2-n1) = a; 244 | tie = tie + 1; 245 | end 246 | end 247 | for k = 1:net.K 248 | slots(IDp(k),sim.M) = 1; 249 | end 250 | 251 | else 252 | slots(IDp(setI),sim.M) = 1; 253 | end 254 | end 255 | 256 | % assign ST agents 257 | count = zeros(1,maxM); 258 | for m = 1:maxM 259 | mpt = 0; 260 | % time step at prediction horizon of PT agents 261 | if(m == sim.M) 262 | mpt = sum(slots(:,m)); 263 | numAvailable = net.K - mpt; 264 | else 265 | numAvailable = net.K; 266 | end 267 | IDs = []; 268 | for h = 1:length(Hm) 269 | if(Hm(h) == m) 270 | count(m) = count(m)+1; 271 | IDs = [IDs IDm(h)]; 272 | end 273 | end 274 | if(count(m) > numAvailable) 275 | I = randperm(length(IDs),numAvailable); 276 | i = IDs(I); 277 | slots(i,m) = 1; 278 | % set flag to switch to PT or recompute M 279 | IDs(I) = []; 280 | slots(IDs,m) = -1; 281 | else 282 | if(~isempty(IDs)) 283 | slots(IDs,m) = 1; 284 | end 285 | end 286 | end 287 | 288 | % for m = 1:maxM 289 | % IDs = []; 290 | % for h = 1:length(H) 291 | % if(H(h) == m) 292 | % count(m) = count(m)+1; 293 | % IDs = [IDs ids(h)]; 294 | % end 295 | % end 296 | % if(count(m) > net.K-prevDiff) 297 | % diff = count(m) - (net.K-prevDiff); 298 | % if(diff >= count(m)) 299 | % 300 | % else 301 | % I = randperm(length(IDs),net.K-prevDiff); 302 | % i = IDs(I); 303 | % slots(i,m) = 1; 304 | % IDs(I) = []; 305 | % slots(IDs,m+1) = 1; 306 | % end 307 | % 308 | % else 309 | % diff = 0; 310 | % if(~isempty(IDs)) 311 | % slots(IDs,m) = 1; 312 | % end 313 | % end 314 | % prevDiff = diff; 315 | % end 316 | 317 | elseif(strcmp(trig_type,'ST2')) 318 | %---------------------------------------------------------------------- 319 | % METHOD 1 - ST: K largest D-step probabilities 320 | %---------------------------------------------------------------------- 321 | if(net.sched_type == 0) 322 | if(sim.N <= net.K) 323 | slots = ones(sim.N,1); 324 | else 325 | slots = zeros(sim.N,ceil((length(H)-(net.K-NAS))/net.K)+1); 326 | % sort based on comm probs 327 | [Hs,ID] = sort(H,'descend'); 328 | ids = ids(ID); 329 | for m = 1:size(slots,2) 330 | if(m == 1) 331 | slotsAvailable = net.K-NAS; 332 | else 333 | slotsAvailable = net.K; 334 | end 335 | if(length(Hs) <= slotsAvailable) 336 | for i = 1:length(Hs) 337 | slots(ids(i),m) = 1; 338 | end 339 | else 340 | [C,~,IC] = unique(Hs,'stable'); 341 | 342 | % if ties occur, randomize 343 | if(length(Hs) ~= length(C)) 344 | if(Hs(net.K-NAS) == Hs(net.K-NAS+1)) 345 | n1 = numel(find(IC(1:net.K-NAS)==IC(net.K-NAS))); 346 | n2 = numel(find(IC==IC(net.K-NAS))); 347 | a = ids(net.K-NAS-n1+1:net.K-NAS+n2-n1); 348 | a = a(randperm(n2)); 349 | ids(net.K-NAS-n1+1:net.K-NAS+n2-n1) = a; 350 | tie = tie + 1; 351 | % ties = length(H)-length(C); 352 | end 353 | end 354 | if(m == 1) 355 | for i = 1:net.K-NAS 356 | slots(ids(i),m) = 1; 357 | end 358 | Hs(1:net.K-NAS) = []; 359 | ids(1:net.K-NAS) = []; 360 | else 361 | for i = 1:net.K 362 | slots(ids(i),m) = 1; 363 | end 364 | Hs(1:net.K) = []; 365 | ids(1:net.K) = []; 366 | end 367 | end 368 | end 369 | end 370 | 371 | %---------------------------------------------------------------------- 372 | % METHOD 2 - ST: Agents w/ prob > h 373 | %---------------------------------------------------------------------- 374 | elseif(net.sched_type == 1) 375 | IDs = ids; 376 | slots = zeros(sim.N,ceil((length(H)-(net.K-NAS))/net.K)+1); 377 | IDg = H > net.h; 378 | IDl = H <= net.h; 379 | Hg = H(IDg); 380 | 381 | % more agents have prob > h 382 | if(length(Hg) > net.K) 383 | % sort based on hitting probabilities 384 | [Hs,ID] = sort(H,'descend'); 385 | ids = ids(ID); 386 | for m = 1:size(slots,2) 387 | if(m == 1) 388 | slotsAvailable = net.K-NAS; 389 | else 390 | slotsAvailable = net.K; 391 | end 392 | if(length(Hs) <= slotsAvailable) 393 | for i = 1:length(Hs) 394 | if(Hs(i) > net.h) 395 | slots(ids(i),m) = 1; 396 | end 397 | end 398 | else 399 | [C,~,IC] = unique(Hs,'stable'); 400 | 401 | % if ties occur, randomize 402 | if(length(Hs) ~= length(C)) 403 | if(Hs(net.K-NAS) == Hs(net.K-NAS+1)) 404 | n1 = numel(find(IC(1:net.K-NAS)==IC(net.K-NAS))); 405 | n2 = numel(find(IC==IC(net.K-NAS))); 406 | a = ids(net.K-NAS-n1+1:net.K-NAS+n2-n1); 407 | a = a(randperm(n2)); 408 | ids(net.K-NAS-n1+1:net.K-NAS+n2-n1) = a; 409 | tie = tie + 1; 410 | % ties = length(H)-length(C); 411 | end 412 | end 413 | if(m == 1) 414 | for i = 1:net.K-NAS 415 | if(Hs(i) > net.h) 416 | slots(ids(i),m) = 1; 417 | end 418 | end 419 | Hs(1:net.K-NAS) = []; 420 | ids(1:net.K-NAS) = []; 421 | else 422 | for i = 1:net.K 423 | if(Hs(i) > net.h) 424 | slots(ids(i),m) = 1; 425 | end 426 | end 427 | Hs(1:net.K) = []; 428 | ids(1:net.K) = []; 429 | end 430 | end 431 | end 432 | % assign agents w/ prob <=h to switch to PT 433 | slots(IDs(IDl),1) = -1; 434 | 435 | else 436 | for i = 1:length(H) 437 | if(H(i) > net.h) 438 | slots(ids(i),1) = 1; 439 | else 440 | % assign agents w/ prob <=h to switch to PT 441 | slots(ids(i),1) = -1; 442 | end 443 | end 444 | end 445 | end 446 | end -------------------------------------------------------------------------------- /source_code/NetworkManager_ET.m: -------------------------------------------------------------------------------- 1 | function [slots] = NetworkManager_ET(sim,net,znorms) 2 | % Scheduler: network slot assignment for time k+M using the Mamduhi paper 3 | % 4 | % sim structure containing the simulation parameters 5 | % net structure containing the network parameters 6 | % H Mstep comm. prob. of all agents 7 | % 8 | % Outputs: 9 | % slots slots allocation for all agents (binary {0,1}) 10 | % ties if a critical tie occurs at the current time step 11 | % (critical is if tie occurs at the Kth slot) 12 | %-------------------------------------------------------------------------- 13 | 14 | P = zeros(1,sim.N); 15 | slots = zeros(1,sim.N); 16 | lambda = 0; % scheduler bound 17 | ids = linspace(1,sim.N,sim.N); 18 | znorms = znorms.^2; 19 | 20 | % priority measure 21 | znorms_afterbound = znorms(znorms > lambda); 22 | ids_afterbound = find(znorms>lambda); 23 | jk = length(ids_afterbound); 24 | 25 | for ID = 1:sim.N 26 | if(znorms(ID) <= lambda) 27 | P(ID) = 0; 28 | else 29 | if(jk <= net.K) 30 | % P(ID) = 1; 31 | slots(ids_afterbound) = 1; 32 | break; 33 | else 34 | P(ID) = znorms(ID)/sum(znorms_afterbound); 35 | end 36 | end 37 | end 38 | 39 | % biased randomization 40 | if(jk > net.K) 41 | for k = 1:net.K 42 | a(k) = randsample(ids,1,true,P); 43 | P(ids == a(k)) = 0; 44 | slots(a(k)) = 1; 45 | end 46 | end -------------------------------------------------------------------------------- /source_code/getExitProbPDE.m: -------------------------------------------------------------------------------- 1 | function getExitProbPDE(sim,file1,file2,folder) 2 | % intermediate function to run the numerical solutions of the 1,2,3D PDE 3 | % 4 | % sim: structure containing simulation parameters 5 | % file1: filename for lookup table [string] 6 | % file2: filename for reset probability [string] 7 | % folder: save location folder name 8 | % 9 | %-------------------------------------------------------------------------- 10 | 11 | setGlobalParams(sim.A,sim.Q,sim.delta,sim.dt); 12 | z = sim.Z0_set; 13 | 14 | runtime = tic; 15 | if(sim.nx == 1) 16 | 17 | if(strcmp(sim.TRIG_TYPE,'ST')) % for ST 18 | t = 0:sim.dt:30*sim.dt; 19 | Sprob = solve_PDE_1D(z,t,sim.TRIG_TYPE); 20 | prob = 1 - Sprob; 21 | 22 | figure; 23 | plot(0:length(t)-1,prob); 24 | xlabel('M'); 25 | ylabel('P(\tau < M\Delta t | Z(0)=0)'); 26 | 27 | elseif(strcmp(sim.TRIG_TYPE,'PT')) % for PT 28 | t = linspace(0,2*sim.M*sim.dt,3); 29 | Sprob = solve_PDE_1D(z,t,sim.TRIG_TYPE); 30 | prob = 1 - Sprob; 31 | prob = prob'; 32 | reset_prob = interp1(z,prob,0); 33 | % figure; 34 | % plot(z,prob); 35 | % xlabel('Error Z'); 36 | % ylabel('P(\tau < M\Delta t | Z(0)=z)'); 37 | end 38 | 39 | 40 | elseif(sim.nx == 2) 41 | 42 | if(strcmp(sim.TRIG_TYPE,'ST')) 43 | t = 0:sim.dt:30*sim.dt; 44 | Sprob = solve_PDE_2D(z,t,sim.TRIG_TYPE); 45 | prob = 1 - Sprob; 46 | 47 | % figure; 48 | % plot(0:length(t)-1,prob); 49 | % xlabel('M'); 50 | % ylabel('P(\tau < M\Delta t | Z(0)=0)'); 51 | 52 | elseif(strcmp(sim.TRIG_TYPE,'PT')) 53 | t = linspace(0,2*sim.M*sim.dt,3); 54 | Sprob = solve_PDE_2D(z,t,sim.TRIG_TYPE); 55 | prob = 1 - Sprob; 56 | 57 | % figure; 58 | % surf(z,z,prob); 59 | % xlabel('Error Z'); 60 | % ylabel('Error Z'); 61 | % zlabel('P(\tau < M\Delta t | Z(0)=z)'); 62 | end 63 | 64 | elseif(sim.nx == 3) 65 | 66 | % t = 0:sim.dt/2e1:2*sim.M*sim.dt; 67 | t = linspace(0,2*sim.M*sim.dt,3); 68 | Sprob = solve_PDE_3D(sim.Z0_set,t); 69 | prob = 1 - Sprob; 70 | 71 | % xslice = [-0.01,0,0.01]; 72 | % lvls = 0:0.1:1; 73 | % [X,Y,Z] = meshgrid(sim.Z0_set,sim.Z0_set,sim.Z0_set); 74 | % figure; 75 | % contourslice(X,Y,Z,prob,xslice,[],[],lvls) 76 | % colorbar 77 | % view(3) 78 | % grid on 79 | else 80 | 81 | end 82 | toc(runtime); 83 | 84 | % Save lookup table 85 | fprintf('\tSaving Numerical lookup table...'); 86 | cd(folder); 87 | save(file1,'prob'); 88 | save(file2,'reset_prob'); 89 | cd('../'); 90 | fprintf('Done\n'); 91 | clear global; 92 | 93 | end 94 | 95 | 96 | function setGlobalParams(Ad,Qd,delta,dt) 97 | global w x y z 98 | w = Ad; 99 | x = Qd; 100 | y = delta; 101 | z = dt; 102 | end 103 | 104 | -------------------------------------------------------------------------------- /source_code/mergeFigures.m: -------------------------------------------------------------------------------- 1 | function mergeFigures() 2 | % merge saved figures into one subplot figure 3 | 4 | disp('---------------------------------------------------------------------'); 5 | disp('note that all desired .fig files should contain only one figure inside it.'); 6 | disp('note that all desired .fig files should be defined in the 2D space.'); 7 | disp('Please press enter to continue'); 8 | pause 9 | % Number of the run 10 | N=0; 11 | answer='y'; 12 | while strcmpi(answer,'y') 13 | N=N+1; 14 | % Get the path and filename of the desired fig file 15 | filename=0; 16 | run=0; 17 | while isequal(filename,0) 18 | if run==2 19 | error('please choose your fig file'); 20 | end 21 | disp('---------------------------------------------------'); 22 | disp('Select the desired fig file which you want to insert in the subplot: '); 23 | [filename,pathname]=uigetfile({'*.fig';'*.FIG'},'Select the .fig file you want to insert in the subplot'); 24 | run=run+1; 25 | end 26 | clear FN 27 | FN=[pathname filename]; 28 | % open figure 29 | h(N) = openfig(FN,'new'); 30 | % get handle to axes of figure 31 | ax(N)=gca; 32 | answer=input('Do you have more .fig files to read? \n','s'); 33 | end 34 | K=input('How many figures in a row do you want to have? \n'); 35 | if isempty(K) 36 | K=2; 37 | end 38 | figure; 39 | for i=1:N 40 | % create and get handle to the subplot axes 41 | s(i) = subplot(ceil(N/K),K,i); 42 | % get handle to all the children in the figure 43 | aux=get(ax(i),'children'); 44 | for j=1:size(aux) 45 | fig(i) = aux(j); 46 | copyobj(fig(i),s(i)); 47 | hold on 48 | end 49 | % copy children to new parent axes i.e. the subplot axes 50 | xlab=get(get(ax(i),'xlabel'),'string'); 51 | ylab=get(get(ax(i),'ylabel'),'string'); 52 | tit=get(get(ax(i),'title'),'string'); 53 | leg=get(get(ax(i),'legend')); 54 | xlabel(xlab);ylabel(ylab);title(tit); 55 | box on; grid on; 56 | end 57 | 58 | 59 | % figure; 60 | % for i = 1:length(varargin) 61 | % a(i) = hgload(varargin{i}); 62 | % h(i) = subplot(length(varargin),1,i); 63 | % copyobj(allchild(get(a(i),'CurrentAxes')),h(i)); 64 | % l(i) = legend(h(i),'LegendForFirstFigure'); 65 | % end -------------------------------------------------------------------------------- /source_code/multi_agent_setup.m: -------------------------------------------------------------------------------- 1 | function sim = multi_agent_setup(varargin) 2 | % Setup function for multi-agent system simulations 3 | % Initializes the model matrices and input matrices for either the 4 | % stochastic processes, autonomous vehicles or cart-pole experiments 5 | 6 | %-------------------------------------------------------------------------- 7 | sim = varargin{1}; 8 | if(nargin >= 2) 9 | sim.N = max(varargin{2}); 10 | 11 | if(nargin > 2 && strcmp(sim.SIM_TYPE,'AV')) 12 | sim.lanes = max(varargin{3}); 13 | sim.Nl = varargin{4}(:,end)'; 14 | end 15 | end 16 | 17 | 18 | %% Stochastic Process Simulation 19 | 20 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 21 | addpath(genpath(strcat(pwd,'/Stochastic Processes'))); 22 | 23 | sim.N = 5; % number of agents 24 | sim.T = 5; % simulation time 25 | 26 | sim.nx = 1; % number of states 27 | sim.nu = 1; % number of inputs 28 | sim.ny = 1; % number of outputs 29 | 30 | fprintf('Initalizing stochastic process models...'); 31 | 32 | % Homogenous system 33 | sim.A = zeros(sim.nx,sim.nx); 34 | sim.x0 = zeros(sim.nx,1,sim.N); 35 | sim.Ahat = zeros(sim.nx,sim.nx,sim.N); 36 | sim.eps = zeros(sim.nx,1); 37 | 38 | % initialize model and noise covariance matrix 39 | % stable case 40 | rss = drss(sim.nx); 41 | sim.A = rss.A; 42 | sim.var = 8.1e-5; 43 | 44 | % unstable case (random) 45 | % sim.A(1:1+size(sim.A,1):end) = 1; 46 | % unstable case 47 | % d = 1.1*ones(sim.nx,1); 48 | % sim.A = diag(d); 49 | % sim.var = 2.5e-5; 50 | 51 | sim.Q = diag(sim.var*ones(sim.nx,1)); % diagonal covariance matrix 52 | sim.eps = sqrt(sim.Q)*randn(sim.nx,1); % N(0,2.5e-5) distribution 53 | 54 | for agent = 1:sim.N 55 | % initialize prediction Ahat matrix 56 | sim.Ahat(:,:,agent) = sim.A; 57 | 58 | % initialize agent initial state 59 | sim.x0(:,:,agent) = randn(sim.nx,1); % N(1,1) distribution 60 | end 61 | clear agent; 62 | 63 | if(strcmp(sim.PROB_TYPE,'CONTROL')) 64 | sim.B = 0.01*ones(sim.nx,sim.nu); 65 | % for i = 1:sim.nu 66 | % sim.B(i,i) = 0.01; 67 | % end 68 | q = 1; r = 0.1; 69 | sim.Q = diag(q*ones(sim.nx,1)); 70 | sim.R = diag(r*ones(sim.nu,1)); 71 | end 72 | 73 | % adjacency matrix for communication 74 | sim.Adj = ones(sim.N,sim.N); % all agents comm. to each other 75 | 76 | %% Heterogenous system 77 | % 78 | % for agent = 1:sim.N 79 | % % initialize process A matrix 80 | % d = sqrt(6.25e-4)*randn(sim.nx,1) + 0.9; % N(0.9, 6.25e-4) distribution for stable A 81 | % sim.A(:,:,agent) = diag(d); 82 | % % initialize prediction Ahat matrix 83 | % r = 0.1*rand(sim.nx,1); 84 | % sim.Ahat = sim.A - diag(r); 85 | % 86 | % % initialize process noise covariance matrix 87 | % sim.eps(:,:,agent) = sqrt(2.5e-5)*randn(sim.nx,1); % N(0,2.5e-5) distribution 88 | % 89 | % % initialize agent initial state 90 | % sim.x0(:,:,agent) = randn(sim.nx,1)+1; % N(1,1) distribution 91 | % end 92 | 93 | fprintf('done\n\n'); 94 | end 95 | 96 | 97 | 98 | %% Autonomous Vehicle Simulation 99 | 100 | if(strcmp(sim.SIM_TYPE, 'AV')) 101 | addpath(genpath(strcat(pwd,'/Autonomous Vehicles'))); 102 | 103 | fprintf('Initalizing vehicle model...'); 104 | % init_av_params; 105 | 106 | sim.T = 120; % simulation time [s] 107 | % sim.T = 450; % simulation time [s] 108 | sim.t = 0:sim.dt:sim.T; % time vector 109 | 110 | if(nargin < 2) 111 | % sim.lanes = 1; % # lanes 112 | sim.lanes = 5; % # lanes 113 | sim.Nl = 8*ones(1,sim.lanes); % # vehicles per lane 114 | sim.N = sum(sim.Nl); % number of agents 115 | end 116 | 117 | sim.lwidth = 3.7; % lane width [m] 118 | ylc = zeros(1,sim.lanes); % lane center positions [m] 119 | for i = 1:sim.lanes 120 | ylc(i) = sim.lwidth/2 + (i-1)*sim.lwidth; 121 | end 122 | 123 | sim.h = 0.7; % time gap [s] 124 | sim.tau = 0.1; % engine driveline time constant [s] 125 | sim.r = 2.5*ones(sim.N,1); % standstill distance [m] (set to 0 if highway simulation) 126 | sim.L = 4*ones(sim.N,1); % vehicle lengths 127 | 128 | % controller gains 129 | kp = 0.2; 130 | kd = 0.7; 131 | kdd = 0; 132 | if((1+kdd)*kd <= kp*sim.tau) 133 | error('Unstable controller'); 134 | end 135 | sim.K = [kp kd kdd]; 136 | 137 | % Assign memory for variables 138 | sim.posx = zeros(sim.N,length(sim.t)); % x-pos matrix (all agents, all times) 139 | sim.posy = zeros(sim.N,length(sim.t)); % y-pos matrix (all agents, all times) 140 | sim.v = zeros(sim.N,length(sim.t)); % velocity matrix (all agents, all times) 141 | sim.a = zeros(sim.N,length(sim.t)); % accel matrix (all agents, all times) 142 | sim.d = zeros(sim.N,length(sim.t)); % distance matrix (all agents, all times) 143 | sim.dr = zeros(sim.N,length(sim.t)); % desired distance (all agents, times) 144 | sim.e = zeros(sim.N,length(sim.t)); % error matrix (all agents, all times) 145 | sim.u = zeros(sim.N,length(sim.t)); % desired acceleration (all agents,times) 146 | 147 | for n = 1:sim.lanes 148 | for i = 1:sim.Nl(n) 149 | ID = sum(sim.Nl(1:n-1)) + i; 150 | sim.posy(ID,:) = ylc(n)*ones(1,length(sim.t)); % y-pos matrix (all agents, all times) 151 | end 152 | end 153 | 154 | sim.pref = zeros(sim.lanes,length(sim.t)); 155 | sim.vref = zeros(sim.lanes,length(sim.t)); 156 | sim.aref = zeros(sim.lanes,length(sim.t)); 157 | sim.adotref = zeros(sim.lanes,length(sim.t)); 158 | sim.uref = zeros(sim.lanes,length(sim.t)); 159 | 160 | sim.var1 = 2.5e-5;%6.4e-5; 161 | sim.var2 = 9e-6;%1e-6; 162 | sim.var3 = eps; 163 | 164 | % adjacency matrix for communication 165 | % sim.Adj = zeros(sim.N); 166 | 167 | % all vehicles in lane communicate to each other 168 | % str = ''; 169 | % for n = 1:sim.lanes 170 | % a{n} = ones(sim.Nl(n)); 171 | % str = [str,'a{',num2str(n),'}']; 172 | % if(n ~= sim.lanes) 173 | % str = [str,',']; 174 | % end 175 | % end 176 | % sim.Adj = eval(['blkdiag(',str,')']); 177 | % sim.deg = sum(sim.Adj,2); 178 | 179 | % distributed gains 180 | % sim.F = 18*ones(1,3);%sim.K; 181 | % sim.G = 0.01; 182 | 183 | % if all gents communicate to each other 184 | % sim.sumAF1 = (1/sim.deg(1))*sim.N*sim.F(1); 185 | % sim.sumAF2 = (1/sim.deg(1))*sim.N*sim.F(2); 186 | % sim.sumAF3 = (1/sim.deg(1))*sim.N*sim.F(3); 187 | % sim.sumAG = (1/sim.deg(1))*sim.N*sim.G; 188 | 189 | % Method with modified PT framework (predict u) - Distributed CACC 190 | % sim.A = [ 0 1 0; 191 | % 0 0 1; 192 | % -(sim.sumAF1/sim.tau) -(sim.sumAF2/sim.tau) -((1+sim.sumAF3)/sim.tau)]; 193 | % 194 | % sim.Au = 1 - (sim.dt/sim.h)*(1-sim.sumAG); 195 | % sim.nx = size(sim.A,2); % system dimension 196 | % sim.nu = 1; 197 | % sim.A = eye(sim.nx) + sim.dt*sim.A; 198 | % 199 | % sim.Q = diag([sim.var1;sim.var3;sim.var2;sim.F(1)*sim.var1+sim.F(2)*sim.var2]); 200 | 201 | 202 | % Method with origianl PT framework (predict full state) - Regular CACC 203 | sim.A = [ 0 1 0 0; 204 | 0 0 1 0; 205 | 0 0 -1/sim.tau 0; 206 | 0 0 0 -1/sim.h]; 207 | sim.B = [0; 0; -1/sim.tau; 1/sim.h]; 208 | sim.Bff = [0; 0; 1/sim.tau; 0]; 209 | sim.nx = size(sim.A,2); % system dimension 210 | sim.nu = size(sim.B,2); 211 | % discrete 212 | sim.A = eye(sim.nx) + sim.dt*sim.A; 213 | sim.B = sim.dt*sim.B; 214 | sim.Bff = sim.dt*sim.Bff; 215 | 216 | % diagonal covariance matrix 217 | sim.Q = diag([sim.var2;sim.var2;sim.var2;kp*sim.var2+kd*sim.var2]); 218 | 219 | 220 | for agent = 1:sim.N 221 | % initialize prediction Ahat matrix 222 | sim.Ahat(:,:,agent) = sim.A; 223 | end 224 | clear agent; 225 | 226 | % Initialize position and velocity of vehicles 227 | v_init = zeros(sim.N,1); % initial vehicle velocities [m/s] 228 | d_init = 5*ones(sim.lanes,1); % initial inter-vehicle distance [m] 229 | 230 | sim.x0 = zeros(sim.nx,1,sim.N); 231 | sim.x = zeros(sim.nx,length(sim.t),sim.N); 232 | sim.xref = zeros(sim.nx,length(sim.t),sim.lanes); 233 | 234 | for n = 1:sim.lanes 235 | for i = 1:sim.Nl(n)+1 236 | if(i == 1) 237 | % initial position of reference vehicle 238 | sim.pref(n,1) = sim.Nl(n)*(d_init(n)+sim.h*sim.v(1,1)+sim.L(1)); 239 | 240 | % set velocity profile for virtual reference vehicle 241 | sim.vref(n,:) = setLeadVelocity(sim.t,sim.dt); 242 | 243 | sim.pref(n,:) = cumtrapz(sim.t,sim.vref(n,:)) + sim.pref(n,1); 244 | sim.aref(n,:) = diff([eps sim.vref(n,:)])/sim.dt; 245 | sim.adotref(n,:) = diff([eps sim.aref(n,:)])/sim.dt; 246 | sim.uref(n,:) = sim.tau*sim.adotref(n,:) + sim.aref(n,:); 247 | sim.xref(4,:,n) = sim.uref(n,:); 248 | 249 | else 250 | ID = sum(sim.Nl(1:n-1)) + (i-1); 251 | sim.v(ID,1) = v_init(ID); 252 | if(sim.v(ID,1) == 0) 253 | sim.dr(ID,1) = sim.r(ID); 254 | else 255 | sim.dr(ID,1) = sim.h*sim.v(ID,1); 256 | end 257 | if(mod(ID,sim.Nl(n)) == 1) 258 | sim.posx(ID,1) = sim.pref(n,1) - d_init(n) - sim.L(ID); 259 | sim.d(ID,1) = sim.pref(n,1) - sim.posx(ID,1) - sim.L(ID); 260 | else 261 | sim.posx(ID,1) = sim.posx(ID-1,1) - d_init(n) - sim.L(ID); 262 | sim.d(ID,1) = sim.posx(ID-1,1) - sim.posx(ID,1) - sim.L(ID); 263 | end 264 | sim.e(ID,1) = sim.d(ID,1) - sim.dr(ID,1); 265 | sim.x0(1,1,ID) = sim.e(ID,1); 266 | sim.x(:,1,ID) = sim.x0(:,1,ID); 267 | end 268 | end 269 | end 270 | 271 | fprintf('Done\n\n'); 272 | end 273 | 274 | 275 | 276 | %% Cart-Pole Simulation 277 | if(strcmp(sim.SIM_TYPE, 'CP')) 278 | addpath(genpath(strcat(pwd,'/Cart-Pole'))); 279 | 280 | sim.N = 10; % number of agents 281 | sim.T = 60; % experiment time [s] 282 | 283 | % simulated agents 284 | if(sim.CP_TYPE) 285 | sim.mass = 89; % mass on pole 286 | 287 | for i = 1:10 288 | file1 = ['A',num2str(i),'_',num2str(sim.mass),'g.mat']; 289 | file2 = ['B',num2str(i),'_',num2str(sim.mass),'g.mat']; 290 | 291 | f = load(file1); 292 | A(:,:,i) = f.A_new; 293 | f = load(file2); 294 | B(:,i) = f.B_new; 295 | end 296 | fclose('all'); 297 | 298 | Ad_sysID = mean(A,3); 299 | Bd_sysID = mean(B,2); 300 | sim.A = Ad_sysID; 301 | sim.B = Bd_sysID; 302 | sim.var = 2.5e-5; 303 | else 304 | load('CP_model.mat'); 305 | sim.A = Ad_model; 306 | sim.B = Bd_model; 307 | sim.var = 2.5e-5; %6.4e-5; %1e-4; 308 | end 309 | sim.nx = size(sim.A,1); % number of states 310 | sim.nu = size(sim.B,2); % number of inpnuts 311 | sim.ny = sim.nx; % number of outputs 312 | 313 | Cd = eye(sim.nx); 314 | Dd = zeros(sim.nx,sim.nu); 315 | sysd = ss(sim.A,sim.B,Cd,Dd,0.001); 316 | sysd_10 = d2d(sysd,0.01); 317 | sim.A = sysd_10.A; 318 | sim.B = sysd_10.B; 319 | 320 | sim.Q = diag(sim.var*ones(sim.nx,1)); % diagonal covariance matrix 321 | 322 | end -------------------------------------------------------------------------------- /source_code/network_setup.m: -------------------------------------------------------------------------------- 1 | function net = network_setup(varargin) 2 | % Funtion to set up the network 3 | % 4 | % Inputs: 5 | % sim structure containing simulation parameters 6 | % net structure containing network parameters 7 | % N number of system agents 8 | % 9 | % Output: 10 | % net updated network structure with bandwidth, capcity and number of 11 | % slots 12 | %-------------------------------------------------------------------------- 13 | 14 | sim = varargin{1}; 15 | net = varargin{2}; 16 | 17 | if(nargin < 3) 18 | Np = sim.N; 19 | else 20 | Np = varargin{3}; 21 | end 22 | 23 | % capacity per time step [bytes] 24 | net.capacity = sim.dt*net.b; 25 | 26 | % number of communication slots available 27 | if(sim.P_CONSTRAINT) 28 | net.K = floor((net.capacity - (4+2)*Np)/(4*sim.nx)); 29 | else 30 | net.K = floor((net.capacity - 4*Np)/(4*sim.nx)); 31 | end -------------------------------------------------------------------------------- /source_code/params.m: -------------------------------------------------------------------------------- 1 | function [sim,net] = params() 2 | % Function to initialize the simulation, framework, and network parameters. 3 | % 4 | % Outputs: sim Simulation, and framework parameters 5 | % net Network parameters 6 | % 7 | %-------------------------------------------------------------------------- 8 | 9 | %-------------------------------------------------------------------------- 10 | % Simulation Parameters 11 | %-------------------------------------------------------------------------- 12 | sim.PROB_TYPE = 'CONTROL'; % problem type {'ESTIMATION','CONTROL'} 13 | sim.TRIG_TYPE = 'ET'; % trigger type {'ET','PT','PPT','ST'} 14 | sim.ET_TYPE = 'ET2'; % ET trigger type {'ET1'=random, 'ET2'=Mamduhi (error-dependent prioritization)} 15 | sim.SIM_TYPE = 'AV'; % simulation type {'STOCHASTIC', 'AV','CP'} 16 | 17 | if(strcmp(sim.SIM_TYPE,'CP')) 18 | sim.INPUT_NOISE = 0; 19 | sim.CP_TYPE = 1; % {0 = Quanser IP02 pend, 1 = new pend} 20 | end 21 | 22 | sim.dt = 0.01; % time step size 23 | 24 | % initialize agent models 25 | sim = multi_agent_setup(sim); 26 | 27 | %-------------------------------------------------------------------------- 28 | % PT Parameters 29 | %-------------------------------------------------------------------------- 30 | sim.M = 2; % prediction horizon 31 | sim.FET_TYPE = 'MC'; % probability soln type {'MC','NUM'} 32 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 33 | sim.delta = 0.02; % error bound 34 | elseif(strcmp(sim.SIM_TYPE,'AV')) 35 | sim.delta = 0.01; 36 | elseif(strcmp(sim.SIM_TYPE,'CP')) 37 | sim.delta = 0.02; 38 | end 39 | 40 | if(strcmp(sim.TRIG_TYPE,'ET')) 41 | sim.P_CONSTRAINT = 0; 42 | sim.CAST_FLAG = 0; 43 | elseif(strcmp(sim.TRIG_TYPE,'PT')) 44 | sim.P_CONSTRAINT = 1; 45 | sim.CAST_FLAG = 1; 46 | elseif(strcmp(sim.TRIG_TYPE,'PPT')) 47 | sim.PT_period = 5; % compute and send prob at each period 48 | sim.P_CONSTRAINT = 1; 49 | sim.CAST_FLAG = 1; 50 | elseif(strcmp(sim.TRIG_TYPE,'ST')) 51 | sim.ub = 0.7; % ST probability upper bound 52 | sim.CAST_FLAG = 1; 53 | end 54 | 55 | %-------------------------------------------------------------------------- 56 | % Network Parameters 57 | %-------------------------------------------------------------------------- 58 | net.K = 20; % # of communication slots 59 | 60 | if(strcmp(sim.TRIG_TYPE,'PT') || strcmp(sim.TRIG_TYPE,'PPT') || strcmp(sim.TRIG_TYPE,'ST') || strcmp(sim.TRIG_TYPE,'ST2')) 61 | if(strcmp(sim.SIM_TYPE,'STOCHASTIC')) 62 | net.sched_type = 1; 63 | net.arbit_type = 2; 64 | net.p = 0.2; 65 | net.d = 1; 66 | elseif(strcmp(sim.SIM_TYPE,'AV')) 67 | net.sched_type = 0; 68 | net.arbit_type = 2; 69 | net.p = 0.2; 70 | net.d = 1; 71 | end 72 | end 73 | 74 | sim.t = 0:sim.dt:sim.T; % time vector 75 | -------------------------------------------------------------------------------- /source_code/plot_CACC_Comparison.m: -------------------------------------------------------------------------------- 1 | % Script to plot CACC comparison between ET1, ET2, ET3 and PT (w/ and w/o 2 | % extension) 3 | % 4 | % ET1 = slots assigned randomly at each timestep 5 | % ET2 = slots assigned to K random agents for all time steps 6 | % ET3 = slots assigned to vehicles at the beginning of the platoons 7 | % PT = slots assigned M-steps in advance based on predictions 8 | % 9 | %-------------------------------------------------------------------------- 10 | 11 | clear; 12 | clc; 13 | 14 | N1 = [20,30,40,50,100]; 15 | N2 = [20,30,40,50]; 16 | K = 20; 17 | trig_type1 = {'ET1','ET2','ET3','PT','PT*'}; 18 | trig_type2 = {'ET1','PT','PT*'}; 19 | mean_error_norm = nan(length(trig_type1),length(N1)); 20 | mean_util = nan(length(trig_type1),length(N1)); 21 | % load data 22 | % for n = 1:length(N2) 23 | for n = 1:length(N1) 24 | file{1} = ['CACC_ET1pred_var2_K',num2str(K),'_N',num2str(N1(n)),'.mat']; 25 | file{4} = ['CACC_PT_K',num2str(K),'_N',num2str(N1(n)),'.mat']; 26 | file{5} = ['CACC_PTextended_S1A3_var2_K',num2str(K),'_N',num2str(N1(n)),'.mat']; 27 | 28 | if(n ~= length(N1)) 29 | file{2} = ['CACC_ET2pred_var2_K',num2str(K),'_N',num2str(N1(n)),'.mat']; 30 | % file{3} = ['CACC_ET3_K',num2str(K),'_N',num2str(N1(n)),'.mat']; 31 | % triggers = [1,2,3,4,5]; 32 | triggers = [1,2,4,5]; 33 | else 34 | triggers = [1,4,5]; 35 | end 36 | 37 | for tr = triggers 38 | load(file{tr}); 39 | 40 | % mean error norms 41 | for ID = 1:sim.N 42 | ebar(tr,ID) = AGENTS(ID).mean_ctrlError_norm(sim); 43 | end 44 | mean_error_norm(tr,n) = mean(ebar(tr,:)); 45 | stdev_error_norm(tr,n) = std(ebar(tr,:)); 46 | 47 | % get utilization 48 | mean_util(tr,n) = mean(net.util); 49 | 50 | clear AGENTS net sim 51 | end 52 | clear file 53 | fclose('all'); 54 | end 55 | 56 | 57 | %% Plot 58 | pos = [759 302 813 645]; 59 | plot_settings(18,1.5); 60 | 61 | fig = figure('Position',pos); 62 | color = [0 0 0]; 63 | set(fig,'defaultAxesColorOrder',[color; color]); 64 | hold on;hold on;grid on; box on; 65 | xlim([15 N1(end)+5]); 66 | xlabel('Number of Agents [N]'); 67 | yyaxis left; 68 | title(['Trigger Comparison, K = ',num2str(K)]); 69 | % plot mean 70 | % plot(N1,mean_error_norm(4,:),'-s','MarkerSize',8,'LineWidth',1.5,'Color',[0 0.4470 0.7410]); 71 | plot(N1,mean_error_norm(5,:),'-s','MarkerSize',8,'LineWidth',1.5,'Color',[0 0.4470 0.7410]); 72 | plot(N1,mean_error_norm(1,:),'-s','MarkerSize',8,'LineWidth',1.5,'Color',[0.85 0.3250 0.0980]); 73 | ylabel('Mean Control Error Norm $\bar{E}$'); 74 | yyaxis right; 75 | plot(N1,mean_error_norm(2,:),'-*','MarkerSize',8,'LineWidth',1.5,'Color',[0.9290 0.6490 0.1250]); 76 | plot(N1,mean_error_norm(3,:),'-*','MarkerSize',8,'LineWidth',1.5,'Color',[0.4940 0.1840 0.5560]); 77 | % plot variance 78 | % plot(N1,stdev_error_norm(4,:),'-s','MarkerSize',8,'LineWidth',1.5,'Color',[0 0.4470 0.7410]); 79 | % plot(N1,stdev_error_norm(5,:),'-s','MarkerSize',8,'LineWidth',1.5,'Color',[0 0.4470 0.7410]); 80 | % plot(N1,stdev_error_norm(1,:),'-s','MarkerSize',8,'LineWidth',1.5,'Color',[0.85 0.3250 0.0980]); 81 | % ylabel('Mean Control Error Norm $\bar{E}$'); 82 | % yyaxis right; 83 | % plot(N1,stdev_error_norm(2,:),'-*','MarkerSize',8,'LineWidth',1.5,'Color',[0.9290 0.6490 0.1250]); 84 | % plot(N1,stdev_error_norm(3,:),'-*','MarkerSize',8,'LineWidth',1.5,'Color',[0.4940 0.1840 0.5560]); 85 | % plot errorbars 86 | % errorbar(N1,mean_error_norm(5,:),stdev_error_norm(4,:),'-s','Color',[0.85 0.3250 0.0980]); 87 | % errorbar(N1,mean_error_norm(1,:),stdev_error_norm(1,:),'-s','Color',[0 0.4470 0.7410]); 88 | % ylabel('Mean Control Error Norm $\bar{E}$'); 89 | % yyaxis right; 90 | % plot(N1,mean_error_norm(2,:),'-*','MarkerSize',8,'LineWidth',1.5,'Color',[0.9290 0.6490 0.1250]); 91 | % errorbar(N,mean_error_norm(2,:),stdev_error_norm(2,:),'-*','Color',[0.9290 0.6490 0.1250]); 92 | % errorbar(N,mean_error_norm(3,:),stdev_error_norm(3,:),'-*','Color',[0.4940 0.1840 0.5560]); 93 | temp = {'PT','ET1','ET2','ET3'}; 94 | l = legend(temp); 95 | l.Orientation = 'horizontal'; 96 | l.Location = 'northwest'; 97 | 98 | figure('Position',pos); 99 | hold on;grid on; box on; 100 | % plot(N1,mean_util(4,:),'-s','LineWidth',1.5); 101 | plot(N1,mean_util(5,:),'-s','LineWidth',1.5); 102 | plot(N1,mean_util(1,:),'-s','LineWidth',1.5); 103 | plot(N1,mean_util(2,:),'-s','LineWidth',1.5); 104 | plot(N1,mean_util(3,:),'-s','LineWidth',1.5); 105 | % for tr = 1:length(trig_type) 106 | % plot(N,temp(tr,:),'-s','LineWidth',1.5); 107 | % end 108 | xlabel('Number of Agents [N]'); 109 | ylabel('Mean Network Utilization $\bar{U}$ [$\%$]'); 110 | title(['Trigger comparison, K = ',num2str(K)]); 111 | l = legend(temp); 112 | l.Orientation = 'horizontal'; 113 | l.Location = 'northwest'; 114 | xlim([15 N1(end)+5]); -------------------------------------------------------------------------------- /source_code/plot_settings.m: -------------------------------------------------------------------------------- 1 | function plot_settings(f,l) 2 | % Funtion to setup the plot setting 3 | % 4 | % Inputs: 5 | % f font size 6 | % l plot line width 7 | % 8 | %-------------------------------------------------------------------------- 9 | 10 | set(groot,'defaultLineLineWidth','remove'); 11 | set(0,'DefaultAxesFontSize','remove'); 12 | set(0,'defaultLegendInterpreter','remove'); 13 | set(0,'defaultTextInterpreter','remove'); 14 | set(0,'defaultAxesTickLabelInterpreter','remove'); 15 | fontSize = f; 16 | lineWidth = l; 17 | grayColor = [0.8 0.8 0.8]; 18 | set(0,'defaultLegendInterpreter','latex'); 19 | set(0,'defaultTextInterpreter','latex'); 20 | set(0,'DefaultAxesFontSize',fontSize); -------------------------------------------------------------------------------- /source_code/run_Monte_Carlo.m: -------------------------------------------------------------------------------- 1 | function run_Monte_Carlo(sim, file1,file2, folder) 2 | % Function to run a Monte Carlo simulation of the error process 3 | % 4 | % sim: structure containing simulation parameters 5 | % file1: filename for lookup table [string] 6 | % file2: filename for reset probability [string] 7 | % folder: save location folder name 8 | % 9 | %-------------------------------------------------------------------------- 10 | 11 | B = sim.numSim; 12 | 13 | Ac = (1/sim.dt)*(sim.A - eye(sim.nx)); 14 | Qc = sqrt(sim.Q)/sqrt(sim.dt); 15 | if(sim.INPUT_NOISE) 16 | Bc = sim.B/sim.dt; 17 | end 18 | 19 | fprintf('Monte Carlo Simulation in progress...'); 20 | tMC = tic; 21 | 22 | % eps = Qc*randn(sim.nx,sim.M,B); 23 | W = sqrt(sim.dt)*randn(sim.nx,sim.M,B); 24 | 25 | prob = cell(sim.nx,1); 26 | [prob{:}] = ndgrid(zeros(1,length(sim.Z0_set))); 27 | prob = prob{1}; 28 | 29 | reset_prob = zeros(1,sim.M); 30 | 31 | v = ones(1,sim.nx); % index vector 32 | ready = false; 33 | reverseStr = ''; 34 | % P = zeros(size(prob),M); 35 | % for m = 1:M 36 | while ~ready 37 | V = num2cell(v); 38 | index = sub2ind(size(prob),V{:}); 39 | 40 | z0 = sim.Z0_set(v)'; 41 | if(norm(z0) >= sim.delta) 42 | prob(index) = 1; 43 | else 44 | count = 0; 45 | 46 | for b = 1:B 47 | z_prev = z0; 48 | 49 | %-- Stopping Times --% 50 | if(strcmp(sim.MC_type,'STO')) 51 | % run error process M steps and if its norm exceeds delta 52 | % at any point increment count 53 | for k = 1:sim.M 54 | % update error process 55 | if(sim.INPUT_NOISE) 56 | % z = sim.A*z_prev + sim.B*eye(sim.nx)*eps(:,k,b); 57 | z = sim.A*z_prev + Qc*Bc; 58 | else 59 | z = (eye(size(Ac,1)) + Ac*sim.dt)*z_prev + Qc*W(:,k,b); 60 | end 61 | % z = sim.A*z_prev + eps(:,k,b); 62 | z_prev = z; 63 | if(norm(z) >= sim.delta) 64 | count = count + 1; 65 | break; 66 | end 67 | end 68 | 69 | %-- Error --% [MAY NOT NEED] 70 | elseif(strcmp(sim.MC_type,'ERR')) 71 | % only checks error after M steps 72 | for k = 1:sim.M 73 | % update stochastic error process 74 | eps = sqrt(sim.Q)*randn(sim.nx,1); 75 | z = sim.A*z_prev + eps; 76 | z_prev = z; 77 | end 78 | if(norm(z) >= sim.delta) 79 | count = count + 1; 80 | end 81 | end 82 | end 83 | prob(index) = count/B; 84 | % if(norm(z0) == 0) 85 | % reset_prob(sim.M) = count/B; 86 | % end 87 | end 88 | 89 | % n-for loops tracked by index vector 90 | ready = true; 91 | for m = 1:sim.nx 92 | v(m) = v(m) + 1; 93 | if(v(m) <= length(sim.Z0_set)) 94 | ready = false; 95 | break; 96 | end 97 | v(m) = 1; 98 | end 99 | 100 | % display percentage completed 101 | percentDone = floor(100*index/numel(prob)); 102 | msg = sprintf('%i', percentDone); 103 | fprintf([reverseStr, msg]); 104 | fprintf('%%'); 105 | reverseStr = repmat(sprintf('\b'), 1, length(msg)+1); 106 | 107 | end 108 | clear eps 109 | runtime = toc(tMC); 110 | fprintf([reverseStr, 'Done\n\n']); 111 | fprintf(['\tMonte Carlo completed in ',num2str(runtime),' seconds\n\n']); 112 | 113 | 114 | % reset probabilities: P(tau < Mdt| Z= 0) 115 | if(sim.M > 1) 116 | for m = 1:sim.M 117 | count = 0; 118 | for b = 1:B 119 | z_prev = zeros(sim.nx,1); 120 | 121 | %-- Stopping Times --% 122 | if(strcmp(sim.MC_type,'STO')) 123 | % run error process M steps and if its norm exceeds delta 124 | % at any point increment count 125 | for k = 1:m 126 | % update stochastic error process 127 | % eps = sqrt(sim.Q)*randn(sim.nx,1); 128 | % z = sim.A*z_prev + eps; 129 | z = (eye(size(Ac,1)) + Ac*sim.dt)*z_prev + Qc*W(:,k,b); 130 | z_prev = z; 131 | if(norm(z) >= sim.delta) 132 | count = count + 1; 133 | break; 134 | end 135 | end 136 | 137 | %-- Error --% 138 | elseif(strcmp(sim.MC_type,'ERR')) 139 | % only checks error after M steps 140 | for k = 1:m 141 | % update stochastic error process 142 | eps = sqrt(sim.Q)*randn(sim.nx,1); 143 | z = sim.A*z_prev + eps; 144 | z_prev = z; 145 | end 146 | if(norm(z) >= sim.delta) 147 | count = count + 1; 148 | end 149 | end 150 | end 151 | reset_prob(m) = count/B; 152 | end 153 | end 154 | 155 | % Store and save lookup table 156 | fprintf('\tSaving Monte Carlo lookup table...'); 157 | cd(folder); 158 | save(file1,'prob'); 159 | save(file2,'reset_prob'); 160 | cd('../'); 161 | fprintf('Done\n'); -------------------------------------------------------------------------------- /source_code/solve_PDE_1D.m: -------------------------------------------------------------------------------- 1 | function survival_prob = solve_PDE_1D(z,t,type) 2 | % Funtion to solve the 1D PDE 3 | % 4 | % Inputs: 5 | % x vector of spatial values at which solution should be computed 6 | % t vector of time values at which solution should be computed 7 | % type trigger type 8 | % 9 | % Output: 10 | % survival_prob solution of PDE (ie. survival probability) 11 | % 12 | %-------------------------------------------------------------------------- 13 | 14 | m = 0; 15 | 16 | % computes solution for all T in [0,2*Mdt] 17 | % pde_sol = pdepe(m,@pdeEqn,@pdeIC,@pdeBC,x,t); 18 | % 19 | % figure; 20 | % surf(x,t,1-pde_sol); 21 | % xlabel('Distance x'); 22 | % ylabel('Time t'); 23 | 24 | mstep_sol = pdepe(m,@pdeEqn,@pdeIC,@pdeBC,z,t); 25 | 26 | if(strcmp(type,'ST')) 27 | % get solution for t = M*dt for M in (0,20) at z=0 28 | survival_prob = mstep_sol(:,ceil(end/2)); 29 | 30 | elseif(strcmp(type,'PT')) 31 | % get solution for t = M*dt 32 | [survival_prob,~] = pdeval(m,z,mstep_sol(2,:),z); 33 | end 34 | 35 | end 36 | 37 | 38 | % -------------------------------------------------------------- 39 | function [c,f,s] = pdeEqn(x,t,u,DuDx) 40 | [Ad,Qd,~,dt] = getModelParams; 41 | Ac = (1/dt)*(Ad - eye(size(Ad,1))); 42 | Qc = sqrt(Qd(1,1)/dt); 43 | c = 1; 44 | f = 0.5*Qc^2*DuDx; 45 | s = Ac*x*DuDx; 46 | end 47 | % -------------------------------------------------------------- 48 | function u0 = pdeIC(x) 49 | u0 = 1; 50 | end 51 | % -------------------------------------------------------------- 52 | function [pl,ql,pr,qr] = pdeBC(xl,ul,xr,ur,t) 53 | pl = ul; 54 | ql = 0; 55 | pr = ur; 56 | qr = 0; 57 | end 58 | 59 | 60 | function [Ad,Qd,delta,dt] = getModelParams 61 | global w x y z 62 | Ad = w; 63 | Qd = x; 64 | delta = y; 65 | dt = z; 66 | end -------------------------------------------------------------------------------- /source_code/solve_PDE_2D.m: -------------------------------------------------------------------------------- 1 | function survival_prob = solve_PDE_2D(z,t,type) 2 | % Funtion to solve the 2D PDE 3 | % 4 | % Inputs: 5 | % z vector of spatial values at which solution should be computed 6 | % t vector of time values at which solution should be computed 7 | % type trigger type 8 | % 9 | % Output: 10 | % survival_prob solution of PDE (ie. survival probability) 11 | % 12 | %-------------------------------------------------------------------------- 13 | 14 | [Ad,Qd,delta,dt] = getModelParams; 15 | Ac = (1/dt)*(Ad - eye(size(Ad,1))); 16 | Qc = sqrt(Qd)/sqrt(dt); 17 | 18 | model = createpde(1); 19 | geometryFromEdges(model,@circlefunction); 20 | 21 | % pdegplot(@circlefunction,'EdgeLabels','on','FaceLabels','on'); 22 | 23 | % PDE coefficients [TODO] 24 | c1 = 0.5*(Qc(1,1)^2 + Qc(1,2)^2); 25 | c2 = Qc(1,1)*Qc(2,1); 26 | c3 = Qc(1,2)*Qc(2,2); 27 | c4 = 0.5*(Qc(2,1)^2 + Qc(2,2)^2); 28 | c = [c1 c2 c3 c4]'; 29 | f = @(location,state) location.x.*(Ac(1,1).*state.ux + Ac(2,1).*state.uy) ... 30 | + location.y.*(Ac(1,2).*state.ux + Ac(2,2).*state.uy); 31 | 32 | specifyCoefficients(model,'m',0,... 33 | 'd',1,... 34 | 'c',c,... 35 | 'a',0,... 36 | 'f',f); 37 | 38 | % Initial Conditions 39 | setInitialConditions(model,1); 40 | 41 | % Boundary Conditions 42 | applyBoundaryCondition(model,'dirichlet','Edge',1:4,'u',0); 43 | 44 | % Mesh 45 | hmax = delta/20; % element size 46 | mesh = generateMesh(model,'Hmax',hmax); 47 | % pdeplot(model); 48 | 49 | % Solver options 50 | model.SolverOptions.RelativeTolerance = 1.0e-3; 51 | model.SolverOptions.AbsoluteTolerance = 1.0e-4; 52 | 53 | sol = solvepde(model,t); 54 | 55 | 56 | if(strcmp(type,'ST')) 57 | % get solution for t = M*dt for M in (0,20) at z=0 58 | survival_prob = 1; 59 | 60 | elseif(strcmp(type,'PT')) 61 | % get value at Mdt 62 | [X,Y] = meshgrid(z,z); 63 | survival_prob = interpolateSolution(sol,X,Y,2); 64 | survival_prob = reshape(survival_prob,size(X)); 65 | end 66 | 67 | 68 | 69 | 70 | 71 | % u = sol.NodalSolution; 72 | % figure; 73 | % pdeplot(model,'XYData',u(:,2),'Contour','on','ColorMap','jet'); 74 | % title(sprintf('Temperature In The Plate, Transient Solution( %d seconds)\n', ... 75 | % t(1,2))); 76 | % xlabel 'X-coordinate, meters' 77 | % ylabel 'Y-coordinate, meters' 78 | % axis equal; 79 | 80 | end 81 | 82 | 83 | 84 | % Create circular boundary condition using four segments. 85 | function [x,y] = circlefunction(bs,s) 86 | [~,~,r] = getModelParams; 87 | switch nargin 88 | case 0 89 | x = 4; % four edge segments 90 | return 91 | case 1 92 | A = [0,pi/2,pi,3*pi/2; % start parameter values 93 | pi/2,pi,3*pi/2,2*pi; % end parameter values 94 | 1,1,1,1; % region label to left 95 | 0,0,0,0]; % region label to right 96 | x = A(:,bs); % return requested columns 97 | return 98 | case 2 99 | x = r*cos(s); 100 | y = r*sin(s); 101 | end 102 | end 103 | 104 | function [Ad,Qd,delta,dt] = getModelParams 105 | global w x y z 106 | Ad = w; 107 | Qd = x; 108 | delta = y; 109 | dt = z; 110 | end -------------------------------------------------------------------------------- /source_code/solve_PDE_3D.m: -------------------------------------------------------------------------------- 1 | function survival_prob = solve_PDE_3D(z,t) 2 | % Funtion to solve the 3D PDE 3 | % 4 | % Inputs: 5 | % z vector of spatial values at which solution should be computed 6 | % t vector of time values at which solution should be computed 7 | % 8 | % Output: 9 | % survival_prob solution of PDE (ie. survival probability) 10 | % 11 | %-------------------------------------------------------------------------- 12 | 13 | [Ad,Qd,delta,dt] = getModelParams; 14 | Ac = (1/dt)*(Ad - eye(size(Ad,1))); 15 | Qc = sqrt(Qd)/sqrt(dt); 16 | 17 | model = createpde(1); 18 | 19 | gm = multisphere(delta); 20 | model.Geometry = gm; 21 | % pdegplot(model,'CellLabels','on'); 22 | 23 | % PDE coefficients [TODO] 24 | c1 = 0.5*(Qc(1,1)^2 + Qc(1,2)^2 + Qc(1,3)^2); 25 | c2 = Qc(1,1)*Qc(2,1); 26 | c3 = Qc(1,1)*Qc(3,1); 27 | c4 = Qc(1,2)*Qc(2,2) + Qc(1,3)*Qc(2,3); 28 | c5 = 0.5*(Qc(2,1)^2 + Qc(2,2)^2 + Qc(2,3)^2); 29 | c6 = Qc(2,1)*Qc(3,1); 30 | c7 = Qc(1,2)*Qc(3,2) + Qc(1,3)*Qc(3,3); 31 | c8 = Qc(2,2)*Qc(3,2) + Qc(2,3)*Qc(3,3); 32 | c9 = 0.5*(Qc(3,1)^2 + Qc(3,2)^2 + Qc(3,3)^2); 33 | c = [c1 c2 c3 c4 c5 c6 c7 c8 c9]'; 34 | f = @(location,state) location.x.*(Ac(1,1).*state.ux + Ac(2,1).*state.uy + Ac(3,1).*state.uz) ... 35 | + location.y.*(Ac(1,2).*state.ux + Ac(2,2).*state.uy + Ac(3,2).*state.uz) ... 36 | + location.z.*(Ac(1,3).*state.ux + Ac(2,3).*state.uy + Ac(3,3).*state.uz); 37 | 38 | specifyCoefficients(model,'m',0,... 39 | 'd',1,... 40 | 'c',c,... 41 | 'a',0,... 42 | 'f',f); 43 | 44 | % Initial Conditions 45 | setInitialConditions(model,1); 46 | 47 | % Boundary Conditions 48 | applyBoundaryCondition(model,'dirichlet','Face',1,'u',0); 49 | 50 | % Mesh 51 | hmax = delta/20; % element size 52 | mesh = generateMesh(model,'Hmax',hmax); 53 | % pdeplot(model); 54 | 55 | % Solver options 56 | model.SolverOptions.RelativeTolerance = 1.0e-3; 57 | model.SolverOptions.AbsoluteTolerance = 1.0e-4; 58 | 59 | f = exist('pde3D.mat','file'); 60 | if(~f) 61 | tPDE = tic; 62 | fprintf('Starting PDE Solver\n'); 63 | 64 | sol = solvepde(model,t); 65 | runtime = toc(tPDE); 66 | else 67 | load('pde3D.mat'); 68 | end 69 | 70 | [X,Y,Z] = meshgrid(z,z,z); 71 | % get value at Mdt 72 | survival_prob = interpolateSolution(sol,X,Y,Z,2); 73 | survival_prob = reshape(survival_prob,size(X)); 74 | 75 | fprintf(['\tNumerical completed in ',num2str(runtime),' seconds\n\n']); 76 | 77 | end 78 | 79 | 80 | 81 | function [Ad,Qd,delta,dt] = getModelParams 82 | global w x y z 83 | Ad = w; 84 | Qd = x; 85 | delta = y; 86 | dt = z; 87 | end --------------------------------------------------------------------------------