%      PlatoonExercise.M
%
%   Analysis of a string of vehicles with 
%   distance control (ACC)
%
%
% J. Lunze
% 15.3.2018
%
% Version of 23.10.2019
% for 2nd edition: 25.7.2021
% 
%
echo off
clear
close all

%%    I vehicles
%
%  input: velocity reference signal v_i-1
%  output: velocity v_i
%  plant: I
%  local controller: PD
%  controlled vehicle: V_i(s) = Gibar V_i-1(s)
%  with Gibar = (-kDs -kP)/(s^2(1-kD beta) - s(kP beta+kD) -kP)
%
figure(1)
kD=0;
kP=-1;
beta=sqrt(2);
Tend=9;
ZGbar=[-kD -kP];
NGbar=[1-kD*beta -(kP*beta+kD) -kP];
VehicleG=tf(ZGbar, NGbar);
ZFbar=[(1-kD*beta) -kP*beta];
VehicleF=tf(ZFbar, NGbar);
%
[G, Time]=impulse(VehicleG, Tend);
subplot(4,1,[1 2])
plot(Time, G, 'b--', [0 Tend],[0 0],'k:');
hold on
latexylabel('$$\bar{g}_i$$');
rotateY;
%latexxlabel('$$t$$');
xleer;
latextitle('$${\tt PlatoonExercise:}$$ Impulse response of PT2-Approximation');
axis([0 Tend -0.1 0.5]);
yticks([0 0.2 0.4]);
xticksempty([0 2 4 6 8 10]);
%    Fibar
[F, Time]=impulse(VehicleF, Tend);
subplot(4,1,[3 4])
plot(Time, F, 'b--', [0 Tend],[0 0],'k:');
hold on
latexylabel('$$\bar{f}_i$$');
rotateY;
latexxlabel('$$t$$');
axis([0 Tend -0.2 1.1]);
yticks([0 0.5 1]);
xticks([0 2 4 6 8 10]);
%
%   for comparison: the same vehicles with beta=2
beta=2;
NGbar2=[1-kD*beta -(kP*beta+kD) -kP];
VehicleG2=tf(ZGbar, NGbar2);
ZFbar2=[(1-kD*beta) -kP*beta];
VehicleF2=tf(ZFbar2, NGbar2);
%
[G2, T2]=impulse(VehicleG2, Tend);
subplot(4,1,[1 2])
plot(T2, G2, 'b');
hold off
[F2, T2]=impulse(VehicleF2, Tend);
subplot(4,1,[3 4])
plot(T2, F2, 'b');
hold off



%%  Platoon of PT2-vehicles (not externally positive)
%    w(t)= 8    (set-point for leader velocity)
figure(2)
N1=10;
w=8;   % m/s
alpha=1;   % m
length=5;  % vehicle length 5m
Tend=25;
dt=0.03;
Time=[0:dt:Tend]';
%  initial distances
for k1=1:N1
    D0(k1)=k1*alpha;
end
Integrator=tf(1, [1 0]);
V0=w*step(VehicleG, Time);
V=V0;
Vpred=V0;                                    % velocity of predecessor
S0=w*step(VehicleG*Integrator, Time);
S=S0;
Spred=S0;                                    % position of predecessor
D=zeros(size(Time));                         % no distance d_0
for k1=1:N1
    V(:,k1+1)=lsim(VehicleG,Vpred,Time);
    D(:,k1+1)=lsim(VehicleF,Vpred,Time)+alpha;
    S(:,k1+1)=Spred-length-D(:,k1+1);
    Vpred=V(:,k1+1);        %  new predecessor velocity
    Spred=S(:,k1+1);
end
%  velocities
subplot(4,1,[1 2])
plot(Time, V,'b');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
%latexxlabel('$$t$$');
xleer;
xticksempty([0 10 20 30 40 50]);
latextitle('$${\tt PlatoonExercise:}$$ Platoon with ACC');
axis([0 Tend -0.1 10]);
yticks([0 5 10]);
%xticks([0 2 4 6 8 10]);
%  distances
subplot(4,1,[3 4])
plot(Time, D(:,2:N1), 'b');
hold on
plot([0 Tend],[0 0],'k:');
latexylabel('$$d_i$$ in m');
rotateY;
latexxlabel('$$t$$');
axis([0 Tend 0 18]);
yticks([0 5 10 15]);
xticks([0 10 20 30 40 50]);
hold off


%%    Vehicle positions in the same manoeuvre
%
figure(3)
%  positions
plot(Time, S, 'b');
latexylabel('$$s_i$$ in m');
rotateY;
latexxlabel('$$t$$');
%latextitle('$${\tt StringDistControl:}$$ Platoon with ACC');
axis([0 Tend -100 200]);
yticks([-100 0 100 200 300]);
xticks([0 10 20 30]);
hold off


%%   Acceleration/braking manoeuvre
%     set-point changes for the leading vehicle
figure(4)
N1=40;
dt=0.2;
%  Velocity set-point w(t)
v0=0;        % initial velocity
v1=100/3.6;   % maximum velocity  40 km/h
Tv0=0;
Tv1=4;
Tv2=10;
Tv3=20;
TendS=90;
k0e=1; 
k1e=ceil(Tv1-Tv0)/dt;
k2e=k1e+ceil(Tv2-Tv1)/dt;
k3e=k2e+ceil(Tv3-Tv2)/dt;
k4e=k3e+ceil(TendS-Tv3)/dt;
W=v0+v1*[0*ones(k1e-k0e,1);        % velocity of fictitious front car
             0.7*ones(k2e-k1e,1);      % 0.7
             1*ones(k3e-k2e,1);      % 0.3
             0.08*ones(k4e-k3e+1,1)];   % 0.6
TimeW=[0:dt:(k4e-1)*dt]';
%
S=[];
V=[];
D=[];
V0=lsim(VehicleG, W, TimeW);
V=V0;
Vpred=V0;                                    % velocity of predecessor
S0=lsim(VehicleG*Integrator, W, TimeW);
S=S0;
Spred=S0;                                    % position of predecessor
D=zeros(size(TimeW));                         % no distance d_0
for k1=1:N1
    V(:,k1+1)=lsim(VehicleG,Vpred,TimeW);
    D(:,k1+1)=lsim(VehicleF,Vpred,TimeW)+alpha;
    S(:,k1+1)=Spred-length-D(:,k1+1);
    Vpred=V(:,k1+1);        %  new predecessor velocity
    Spred=S(:,k1+1);
end
%  velocities
subplot(4,1,[1 2]);
plot(TimeW, W,'b--', [0 TendS],[0 0],'k:'); % command to leading vehicle
hold on
plot(TimeW, V,'b');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
%latexxlabel('$$t$$');
xleer;
xticksempty([0 20 40 60 80 100 120]);
%latextitle('$${\tt StringDistControl:}$$ Platoon with ACC');
axis([0 TendS -4 1.1*v1]);
yticks([0 10 20 30 40 50]);
hold off
%
%  distances
subplot(4,1,[3 4]);
plot(TimeW, D(:,2:N1), 'b');
hold on
plot([0 TendS],[alpha alpha],'k:');
latexylabel('$$d_i$$ in m');
rotateY;
latexxlabel('$$t$$');
axis([0 TendS -5 45]);
yticks([1 20 40 60]);
xticks([0 20 40 60 80 100 120]);
hold off
%

%%    Vehicle positions in the same manoeuvre
%
figure(5)
plot(TimeW, S, 'b');
latexylabel('$$s_i$$ in m');
rotateY;
latexxlabel('$$t$$');
axis([0 TendS -250 600]);
yticks([-200 0 200 400 600]);
xticks([0 20 40 60 80 100 120]);
hold off



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  The same manoeuvre for vehicles with beta=2
%
figure(6)
%
S=[];
V=[];
D=[];
V0=lsim(VehicleG2, W, TimeW);
V=V0;
Vpred=V0;                                    % velocity of predecessor
S0=lsim(VehicleG2*Integrator, W, TimeW);
S=S0;
Spred=S0;                                    % position of predecessor
D=zeros(size(TimeW));                         % no distance d_0
for k1=1:N1
    V(:,k1+1)=lsim(VehicleG2,Vpred,TimeW);
    D(:,k1+1)=lsim(VehicleF2,Vpred,TimeW)+alpha;
    S(:,k1+1)=Spred-length-D(:,k1+1);
    Vpred=V(:,k1+1);        %  new predecessor velocity
    Spred=S(:,k1+1);
end
%  velocities
subplot(4,1,[1 2]);
plot(TimeW, W,'b--', [0 TendS],[0 0],'k:'); % command to leading vehicle
hold on
plot(TimeW, V,'b');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
%latexxlabel('$$t$$');
xleer;
%latextitle('$${\tt StringDistControl:}$$ Platoon with ACC');
axis([0 TendS -1 1.1*v1]);
yticks([0 10 20 30 40 50]);
xticksempty([0 20 40 60 80 100 120]);
hold off
%  distances
subplot(4,1,[3 4]);
plot(TimeW, D(:,2:N1), 'b');
hold on
plot([0 TendS],[alpha alpha],'k:');
latexylabel('$$d_i$$ in m');
rotateY;
latexxlabel('$$t$$');
axis([0 TendS -2 60]);
yticks([1 20 40 60]);
xticks([0 20 40 60 80 100 120]);
hold off

%%    Vehicle positions in the same manoeuvre
%
figure(7)
plot(TimeW, S, 'b');
latexylabel('$$s_i$$ in m');
rotateY;
latexxlabel('$$t$$');
axis([0 TendS -250 600]);
yticks([-200 0 200 400 600]);
xticks([0 20 40 60 80 100 120]);
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Platoon exercise
%    Gi(s) = 1/(Ti s + 1)s
%    Kvi   = kvi
%    Gibar = kvi/(Ti s^2 + s + kvi)
%    Fibar = (Ti s + 1)/(Ti s^2 + s + kvi)
%
%  
figure(8)
beta=2;
alpha=1;
length=5;
kv1=1/beta;
%   Vehicles with different time constants
T1=0.2;
G1bar=tf([kv1], [T1 1 kv1]);
F1bar=tf([T1 1],[T1 1 kv1]);
T2=0.5;
G2bar=tf([kv1], [T2 1 kv1]);
F2bar=tf([T2 1],[T2 1 kv1]);
T3=0.7;
G3bar=tf([kv1], [T3 1 kv1]);
F3bar=tf([T3 1],[T3 1 kv1]);
T4=1.1;
G4bar=tf([kv1], [T4 1 kv1]);
F4bar=tf([T4 1],[T4 1 kv1]);
T5=1.3;
G5bar=tf([kv1], [T5 1 kv1]);
F5bar=tf([T5 1],[T5 1 kv1]);
Tend=10;
Time=[0:0.1:Tend];
V1=step(G1bar, Time);
V2=step(G2bar, Time);
V3=step(G3bar, Time);
V4=step(G4bar, Time);
V5=step(G5bar, Time);
subplot(4,1,[1 2]);
plot(Time, [V1 V2 V3 V4 V5], 'b', [0 Tend],[1 1],'k:');
latexylabel('$$v_i\;$$');   %  in $$\frac{\rm m}{\rm s}$$
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend -0.05  1.2]);
yticks([0 0.5 1]);
xticks([0 5 10]);
hold off

%%   Vehicle string with ACC
%
figure(9)
dt=0.1;
%  Velocity set-point w(t)
v0=0;        % initial velocity
v1=100/3.6;   % maximum velocity  100 km/h
Tv0=0;
Tv1=5;
Tv2=25;
Tv3=40;
TendS=90;
k0e=1; 
k1e=ceil(Tv1-Tv0)/dt;
k2e=k1e+ceil(Tv2-Tv1)/dt;
k3e=k2e+ceil(Tv3-Tv2)/dt;
k4e=k3e+ceil(TendS-Tv3)/dt;
W=v0+v1*[0*ones(k1e-k0e,1);        % velocity of fictitious front car
             1*ones(k2e-k1e,1);      % 0.7
             0*ones(k3e-k2e,1);      % 0.3
             0.5*ones(k4e-k3e+1,1)];   % 0.6
TimeW=[0:dt:(k4e-1)*dt]';
%
S=[];
V=[];
D=[];
V0=lsim(G1bar, W, TimeW);
V=V0;
Vpred=V0;                                    % velocity of predecessor
S0=lsim(G1bar*Integrator, W, TimeW);
S=S0;
Spred=S0;                                    % position of predecessor
D=zeros(size(TimeW));                         % no distance d_0
%   Platoon with different vehicles
V1=lsim(G1bar, V0, TimeW);
D1=lsim(F1bar, V0, TimeW);
S1=S0-D1-length-alpha;
%
V2=lsim(G1bar, V1, TimeW);
D2=lsim(F1bar, V1, TimeW);
S2=S1-D2-length-alpha;
%
V3=lsim(G2bar, V2, TimeW);
D3=lsim(F2bar, V2, TimeW);
S3=S2-D3-length-alpha;
%
V4=lsim(G3bar, V3, TimeW);
D4=lsim(F3bar, V3, TimeW);
S4=S3-D4-length-alpha;
%
V5=lsim(G4bar, V4, TimeW);
D5=lsim(F4bar, V4, TimeW);
S5=S4-D5-length-alpha;
%
V6=lsim(G5bar, V5, TimeW);
D6=lsim(F5bar, V5, TimeW);
S6=S5-D6-length-alpha;
%
V7=lsim(G4bar, V6, TimeW);
D7=lsim(F4bar, V6, TimeW);
S7=S6-D7-length-alpha;
%
V8=lsim(G5bar, V7, TimeW);
D8=lsim(F5bar, V7, TimeW);
S8=S7-D8-length-alpha;
%
V9=lsim(G5bar, V8, TimeW);
D9=lsim(F5bar, V8, TimeW);
S9=S8-D9-length-alpha;
%
V10=lsim(G1bar, V9, TimeW);
D10=lsim(F1bar, V9, TimeW);
S10=S9-D10-length-alpha;
%
subplot(4,1,[1 2]);
plot(TimeW, W,'b--', [0 TendS],[0 0],'k:'); % command to leading vehicle
hold on
plot(TimeW, [V1 V2 V3 V4 V5 V6 V7 V8 V9 V10], 'b', [0 TendS],[0 0],'k:');
latexylabel('$$v_i\;$$');  %  in $$\frac{\rm m}{\rm s}$$
rotateY;
%latexxlabel('$$t$$');
xleer;
%latextitle('$${\tt StringDistControl:}$$ Platoon with ACC');
axis([0 TendS -3 1.1*v1]);
yticks([0 10 20 30 40 50]);
xticksempty([0 20 40 60 80 100 120]);
hold off
%
subplot(4,1,[3 4]);
plot(TimeW, [D1 D2 D3 D4 D5 D6 D7 D8 D9 D10], 'b', [0 TendS],[0 0],'k:');
hold on
plot([10 50],beta*[v1 v1],'k:');
latexylabel('$$\tilde{d}_i\;$$');  %    in m
rotateY;
latexxlabel('$$t$$');
axis([0 TendS -5 60]);
yticks([0 20 40 60]);
xticks([0 20 40 60 80 100 120]);
hold off


%%    Vehicle positions in the same manoeuvre
%
figure(10)
plot(TimeW, [S0 S1 S2 S3 S4 S5 S6 S7 S8 S9 S10], 'b');
latexylabel('$$s_i$$ in m');
rotateY;
latexxlabel('$$t$$');
axis([0 TendS -100 1000]);
yticks([-200 0 200 400 600 800 1000]);
xticks([0 20 40 60 80 100 120]);
hold off


%%   Externally positive vehicles
%
figure(11)
%   Vehicles with different time constants
T1=0.2;
G1bar=tf([kv1], [T1 1 kv1]);
F1bar=tf([T1 1],[T1 1 kv1]);
T2=0.5;
G2bar=tf([kv1], [T2 1 kv1]);
F2bar=tf([T2 1],[T2 1 kv1]);
T3=0.1;
G3bar=tf([kv1], [T3 1 kv1]);
F3bar=tf([T3 1],[T3 1 kv1]);
T4=0.3;
G4bar=tf([kv1], [T4 1 kv1]);
F4bar=tf([T4 1],[T4 1 kv1]);
T5=0.4;
G5bar=tf([kv1], [T5 1 kv1]);
F5bar=tf([T5 1],[T5 1 kv1]);
Tend=10;
Time=[0:0.1:Tend];
V1=step(G1bar, Time);
V2=step(G2bar, Time);
V3=step(G3bar, Time);
V4=step(G4bar, Time);
V5=step(G5bar, Time);
subplot(4,1,[1 2]);
plot(Time, [V1 V2 V3 V4 V5], 'b', [0 Tend],[1 1],'k:');
latexylabel('$$v_i\;$$');  %  in $$\frac{\rm m}{\rm s}$$
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend -0.05  1.2]);
yticks([0 0.5 1]);
xticks([0 5 10]);
hold off


%%   Vehicle string with ACC
%
figure(12)
S=[];
V=[];
D=[];
V0=lsim(G1bar, W, TimeW);
V=V0;
Vpred=V0;                                    % velocity of predecessor
S0=lsim(G1bar*Integrator, W, TimeW);
S=S0;
Spred=S0;                                    % position of predecessor
D=zeros(size(TimeW));                         % no distance d_0
%   Platoon with different vehicles
V1=lsim(G1bar, V0, TimeW);
D1=lsim(F1bar, V0, TimeW);
S1=S0-D1-length-alpha;
%
V2=lsim(G1bar, V1, TimeW);
D2=lsim(F1bar, V1, TimeW);
S2=S1-D2-length-alpha;
%
V3=lsim(G2bar, V2, TimeW);
D3=lsim(F2bar, V2, TimeW);
S3=S2-D3-length-alpha;
%
V4=lsim(G3bar, V3, TimeW);
D4=lsim(F3bar, V3, TimeW);
S4=S3-D4-length-alpha;
%
V5=lsim(G4bar, V4, TimeW);
D5=lsim(F4bar, V4, TimeW);
S5=S4-D5-length-alpha;
%
V6=lsim(G5bar, V5, TimeW);
D6=lsim(F5bar, V5, TimeW);
S6=S5-D6-length-alpha;
%
V7=lsim(G4bar, V6, TimeW);
D7=lsim(F4bar, V6, TimeW);
S7=S6-D7-length-alpha;
%
V8=lsim(G5bar, V7, TimeW);
D8=lsim(F5bar, V7, TimeW);
S8=S7-D8-length-alpha;
%
V9=lsim(G5bar, V8, TimeW);
D9=lsim(F5bar, V8, TimeW);
S9=S8-D9-length-alpha;
%
V10=lsim(G1bar, V9, TimeW);
D10=lsim(F1bar, V9, TimeW);
S10=S9-D10-length-alpha;
%
%
subplot(4,1,[1 2]);
plot(TimeW, W,'b--', [0 TendS],[0 0],'k:'); % command to leading vehicle
hold on
plot(TimeW, [V1 V2 V3 V4 V5 V6 V7 V8 V9 V10], 'b');
latexylabel('$$v_i\;$$');  %  in $$\frac{\rm m}{\rm s}$$
rotateY;
%latexxlabel('$$t$$');
xleer;
%latextitle('$${\tt StringDistControl:}$$ Platoon with ACC');
axis([0 TendS -3 1.1*v1]);
yticks([0 10 20 30 40 50]);
%xticks([0 2 4 6 8 10]);
hold off
%
subplot(4,1,[3 4]);
plot(TimeW, [D1 D2 D3 D4 D5 D6 D7 D8 D9 D10], 'b', [0 TendS],[0 0],'k:');
hold on
plot([10 50],beta*[v1 v1],'k:');
latexylabel('$$\tilde{d}_i\;$$');  %  in m
rotateY;
latexxlabel('$$t$$');
axis([0 TendS -5 60]);
yticks([0 20 40 60]);
xticks([0 20 40 60 80 100 120]);
hold off


%%    Vehicle positions in the same manoeuvre
%
figure(13)
plot(TimeW, [S0 S1 S2 S3 S4 S5 S6 S7 S8 S9 S10], 'b');
latexylabel('$$s_i$$ in m');
rotateY;
latexxlabel('$$t$$');
axis([0 TendS -100 1000]);
yticks([-200 0 200 400 600 800 1000]);
xticks([0 20 40 60 80 100 120]);
hold off


%%
%
epsfigc15('PlatoonExercise');
