function Traffic_Flow
% This will simulate TRAFFIC FLOW of n cars on a straight freeway with L
% lanes. Changing lanes is allowed. P is the position and speed vector for
% all n cars of the form P = [x1 y1 s1 k1; x2 y2 s2 k2; ... ; xn yn sn kn]
% [The extent of the freeway (length) is FrEx and k is a parameter for
% each car describing how different they are from the average driver in
% terms of top speed and frequency of lane-changing (driving style)
clf
clear
global n L dt dfmin avgS Frwy
n = 40; % number of cars
L = 4; % number of lanes
dt = 0.15; % time step
dfmin = 2; % minimum distance allowed
Frwy = dfmin*(n/L); % length of freeway
avgS = 0.35; % average speed of cars
P = zeros(n,4); % initialize P
P = initial_positions(P);
P = initial_k(P);
Pprev = P; % P a single time step dt ago
iter = 1;
t = 0;
while t >= 0
dPS = PdotS(P);
PSnew = P + dPS; % update P w.r.t. speed and vertical changes dy
cy = PSnew(1:n,2); cy = mod(cy,Frwy); PSnew(1:n,2) = cy; %vertical PBC
dPL = PdotL(PSnew,Pprev);
Pnew = PSnew + dPL; % update P w.r.t lane changes dx
figure(1)
% plot current car arrangement with vertical PBC
xx = Pnew(1:n,1); yy = Pnew(1:n,2); yy = mod(yy,Frwy);
subplot(1,3,1); plot(xx,yy,'bs','MarkerSize',8);
axis([0 L+1 0 Frwy/3]); title('1/3rd Frwy')
subplot(1,3,2); plot(xx,yy,'bs','MarkerSize',8);
axis([0 L+1 Frwy/3 2*Frwy/3]); title('2/3rd Frwy')
subplot(1,3,3); plot(xx,yy,'bs','MarkerSize',8);
axis([0 L+1 2*Frwy/3 Frwy]); title('3/3rd Frwy')
drawnow
% Prep for another iteration
t = t + dt; iter = iter + 1;
Pprev = P;
P = Pnew;
end
function dPS = PdotS(P)
% This will find how P changes with respect to speed dS and movement along the
% highway dy
dPS = zeros(n,4);
for i = 1:n
Si = P(i,3);
dyi = Si * dt;
dPS(i,2) = dyi;
dSi = speed_change(i,P);
dPS(i,3) = dSi;
end
end
function dSi = speed_change(i,P)
% This will determine the speed Si of the ith car Ci based on the speed of
% the car in front of it and how their distance between has changed
ki = P(i,4);
Si = P(i,3);
smax = ki * avgS;
df = dfront(i,P);
sif = speedinfront(i,df,P);
% If the ith car Ci is within the minimum distance to the car in front,
% then set the speed of Ci to the speed of the car in front or the max
% speed of Ci, whichever is less.
if df < dfmin/2
Sinew = min(smax , 0.8 * sif);
dSi = Sinew - Si;
return
end
% Give two accelerations, a spring acceleration with equilibrium dfmin
% behind the car ahead and a regular acceleration which brings a lonely car
% to full speed smax in 10 seconds
if df < dfmin
k = 3;
end
if df < 2*dfmin && df >= dfmin
k = 2;
end
if df >= 2*dfmin
k = 3;
end
springAccel = -(k * ki) * (dfmin - df);
regAccel = smax/10 * dt;
dSi = springAccel;
if dSi > regAccel
dSi = regAccel;
end
if dSi < -2*regAccel
dSi = -2*regAccel;
end
if Si + dSi > smax
dSi = smax - Si;
end
if Si + dSi < 0
dSi = -Si + smax/2;
end
end
function sif = speedinfront(i,df,P)
% This finds the speed of the car in front of ith car Ci
Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);
sif = inf; % For cars in the front of the pack
% Locate car in front
for j = 1:n
if j ~= i
if P(j,1) == cx
if P(j,2) == cy + df
sif = P(j,3); % Speed of car in front
end
end
end
end
end
function [sifL sifR] = speedinfrontLR(i,P)
% This finds speed of cars in front and to left and right of ith car Ci
Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);
[dfL dfR] = dfrontLR(i,P);
% If ith car Ci is at either edge of the freeway and one of the below
% 'if' loops to find sifL or sifR isn't enacted
sifL = 0;
sifR = 0;
for j = 1:n
% Locate car in front LEFT
if P(j,1) == cx - 1
if P(j,2) == cy + dfL
sifL = P(j,3); % Speed of car in front LEFT
end
end
% Locate car in front RIGHT
if P(j,1) == cx + 1
if P(j,2) == cy + dfR
sifR = P(j,3); % Speed of car in front RIGHT
end
end
end
end
function [sibL sibR] = speedinbackLR(i,P)
% This finds speed of cars in front and to left and right of ith car Ci
Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);
[dbL dbR] = dbackLR(i,P);
% If ith car Ci is at either edge of the freeway and one of the below
% 'if' loops to find sibL or sibR isn't enacted
sibL = 0;
sibR = 0;
for j = 1:n
if j ~= i
% Speed of car in front LEFT
if P(j,1) == cx - 1
if P(j,2) == cy - dbL
sibL = P(j,3);
end
end
% Speed of car in front RIGHT
if P(j,1) == cx + 1
if P(j,2) == cy - dbR
sibR = P(j,3);
end
end
end
end
end
function db = dback(i,P)
% This finds the closest car in back of car Ci
Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);
close = [];
for j = 1:n
if j ~= i
if P(j,1) == cx
if P(j,2) <= cy
close = [P(j,2) close];
end
end
end
end
if isempty(close) == 1 % if there are no cars in back of Ci make
db = 1e6; % dback very large
return
end
db = cy - min(close);
end
function df = dfront(i,P)
% This finds the closest car in front of ith car Ci
Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);
close = [];
for j = 1:n
if j ~= i
if P(j,1) == cx
if P(j,2) >= cy
close = [P(j,2) close];
end
end
end
end
if isempty(close) == 1 % if there are no cars in front of Ci make
df = 1e6; % dfront very large
return
end
df = min(close) - cy;
end
function [dbL dbR] = dbackLR(i,P)
% This finds the closest cars which are behind the car Ci, in the lanes
% left and right to the car Ci
Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);
closeleft = [];
closeright = [];
% Distance from Ci to car to left and back
if cx == 1 %in case Ci is at the edge of the freeway
dbL = 0;
end
if cx ~= 1
for j = 1:n
if j ~= i
if P(j,1) == cx - 1 %car to the left and back
if P(j,2) <= cy
closeleft = [P(j,2) closeleft];
end
end
end
end
dbL = cy - min(closeleft);
if isempty(closeleft) == 1 % if there are no cars in back left of Ci
dbL = 1e6; % make dbleft very large
end
end
% Distance from Ci to car to right and back
if cx == L %in case Ci is at the edge of the freeway
dbR = 0;
end
if cx ~= L
for j = 1:n
if j ~= i
if P(j,1) == cx + 1 %car to the right and back
if P(j,2) <= cy
closeright = [P(j,2) closeright];
end
end
end
end
dbR = cy - min(closeright);
if isempty(closeright) == 1 % if there are no cars in back right of Ci
dbR = 1e6; % make dbright very large
end
end
end
function [dfL dfR] = dfrontLR(i,P)
% Now find distance between car Ci and cars to the right and left and in
% front of Ci
Ci = P(i,1:2);
cx = Ci(1);
cy = Ci(2);
closeleft = [];
closeright = [];
% Distance from Ci to car to left and front
if cx == 1 %in case Ci is at the edge of the freeway
dfL = 0;
end
if cx ~= 1
for j = 1:n
if j ~= i
if P(j,1) == cx - 1 %car to the left and front
if P(j,2) >= cy
closeleft = [P(j,2) closeleft];
end
end
end
end
dfL = min(closeleft) - cy;
if isempty(closeleft) == 1 % if there are no cars in front left of Ci
dfL = 1e6; % make dfL very large
end
end
% Distance from Ci to car to right and front
if cx == L %in case Ci is at the edge of the freeway
dfR = 0;
end
if cx ~= L
for j = 1:n
if j ~= i
if P(j,1) == cx + 1 %car to the right and front
if P(j,2) >= cy
closeright = [P(j,2) closeright];
end
end
end
end
dfR = min(closeright) - cy;
if isempty(closeright) == 1 % if there are no cars in front right of Ci
dfR = 1e6; % make dfR very large
end
end
end
function dPL = PdotL(P,Pprev)
% This will find how P changes with respect to lane changes dx. We follow
% the convention that the speed dS and position dy adjustments have been
% made already
dPL = zeros(n,4);
for i = 1:n
Si = P(i,3);
dxi = lane_change(i,Si,P,Pprev);
dPL(i,1) = dxi;
end
end
function dxi = lane_change(i,Si,P,Pprev)
% This will determine whether the ith car Ci will change lanes based on
% the speed of cars around and their distances from Ci
dxi = 0; % Default is not to change lanes
ki = P(i,4); % Recall ki is from 0.50 to 1.50 and determines how
% quick a driver is
smax = ki * avgS; % Desired speed of ith car
% If the left and right cars in back are/were too close, Ci shouldn't
% change lanes
[dbL dbR] = dbackLR(i,P);
[dbLp dbRp] = dbackLR(i,Pprev);
if dbL < dfmin && dbR < dfmin || dbLp < dfmin && dbRp < dfmin
return
end
% If the left and right cars in front are/were too close, Ci shouldn't
% change lanes
[dfL dfR] = dfrontLR(i,P);
[dfLp dfRp] = dfrontLR(i,Pprev);
if dfL < dfmin && dfR < dfmin || dfLp < dfmin && dfRp < dfmin
return
end
% If the speed of the car in front is/was faster than the right and left
% cars in front, Ci shouldn't change lanes
df = dfront(i,P);
sif = speedinfront(i,df,P);
[sifL sifR] = speedinfrontLR(i,P);
f = [sifL sifR sif];
speedF = sort(f);
if sif == speedF(3);
return
end
% If the left and right cars in back are going too fast, Ci shouldn't
% change lanes
[sibL sibR] = speedinbackLR(i,P);
if 1.2 * Si < sibL && 1.2 * Si < sibR
return
end
% For the case where the above isn't the case and Ci wants to change lanes
% because it is getting close to the car in front, ie it will reach the car
% in front within 10 seconds, or because it is being tailgated
db = dback(i,P);
sifon = inf; % anything > smax
if df < dfmin
sifon = sif; % declare speed of 'nearby' cars in front
end
if sifon < smax || df < dfmin || db < dfmin/2
r = rand();
if r < 0.95 % preference to not change lanes, notice
return % higher ki means more lane changes
end
Ldxi = 0;
if sibL <= 1.2 * Si
Ldxi = -1; % lane change to the left
end
if dbL < dfmin || dfL < dfmin || dbLp < dfmin || dfLp < dfmin
Ldxi = 0; % in case there's no room, cancel it
end
Rdxi = 0;
if sibR <= 1.2 * Si
Rdxi = +1; % lane change to the right
end
if dbR < dfmin || dfR < dfmin || dbRp < dfmin || dfRp < dfmin
Rdxi = 0; % in case there's no room, cancel it
end
if sifL > sifR % decide which lane change is better
dxi = Ldxi;
end
if sifR > sifL
dxi = Rdxi;
end
end
end
function P = initial_k(P)
% This fills in ki (driving style) of ith car Ci with ki in interval
% [0.5,1.5]. Most cars will have the average 1, less will have 0.50 and
% 1.50, describing more timid and more aggressive drivers. About 1/2 of
% cars have k ~ 1 , 1/4 have k < 1 and 1/4 have k > 1 Basically, k will
% determine the top speed and frequency of lane-changing for a car
% Set up distribution
kS = linspace(-(0.25)^(1/5),+(0.25)^(1/5),n);
ki = 1 + (kS).^5;
r = randperm(n);
for i = 1:n
j = r(i); %randomly distribute k's among n cars
P(i,4) = ki(j);
end
end
function P = initial_positions(P)
% This will place the n cars in the freeway with L lanes in checkerboard
% fashion
j = 1;
for i = 1:2:n-1
rx = j;
ry = dfmin * ceil(i/L);
P(i,1) = rx;
P(i,2) = ry;
P(i+1,1) = rx + 1;
P(i+1,2) = ry + dfmin/2;
j = 1 + mod (j + 1 , L);
end
end
end