%      TrucksSelforg.M
%
%   Selforganised selection of the leader in 
%   a truck platoon
%   with first-order trucks -> algebraic loop in distance control
%                           -> only one parameter to select delay of truck
%
% J. Lunze
% 10.8.2018
%
% Version of 27.11.2018
% for 2nd edition: 1.5.2021
% 
close all
clear all

%%   Truck parameters
trucklength=18;      % truck length in m
m=20000;        % truck mass in kg
T=13.5;         % time constant of truck acceleration in s
c=1/T;
vmax=90/3.6;    % maximum velocity of trucks at German highways in m/s
vmaxplus=vmax*1.05;  % tolerable maximum velocity for distance control
alpha=3;        % minimum distance of trucks in m
beta=2;         % required time-headway in s
gamma=0.9;      % reduces the estimated maximum speed
g=9.81;         % acceleration of gravity
amax=c*30;      % maximum acceleration to reach max. velocity of 30 m/s=108 km/h
smax=5500;      % maximum position considered in m
v0=90/3.6;      % initial velocity in m/s
s0=0;           % initial position of first vehicle in m
Ts=0.2;         % sampling time for simulation in s

%%  Road profile
%    use function roadprofile.m to determine the slope d for given
%    position s
%    d = sin alpha with alpha = slope angle
%    draw the profile
figure(1)
deltas=10;      % step size for the position
S=[0:deltas:smax];
% Parameters of the function roadprofile.m
p0=0;       % pi = landmarks for changes in the profile in m
p1=500;
p2=700;
p3=1300;
p4=1500;
p5=2900;
p6=3100;
p7=4000;
p8=4200;
Points=[p1; p2; p3; p4; p5; p6; p7; p8];
NoPoints=length(Points);
alpha1=0.04;   %  alphai = slope  0.05 = 5% = height/road length
alpha2=0.10;
alpha3=-0.07;
H=[];           % height of road: h_i
Slope=[];       % slope of road: p_i
h0=0;           % initial height
k1=1;
H(1)=h0;
Slope(1)=0;
for s=[deltas:deltas:smax]
    k1=k1+1;
    Slope(k1)=roadprofile(s);
    H(k1)=H(k1-1)+roadprofile(s)*deltas;
end
subplot(4,1,[1 2])
%plot(S, H,'b');
fill([S S(end) 0], [H 0 0],[1 1 1]/1.2);  % divide by 1 for white
hold on
for k1=1:NoPoints
    plot(Points(k1), H(Points(k1)/deltas+1), 'ko');
end
latexylabel('$$h$$ in m');
rotateY;
xleer;
axis([0 smax -10 220]);
yticks([0 50 100 150 200]);
xticksempty([0 1000 2000 3000 4000 5000]);
latextitle('$${\tt TruckSelforg:}$$ Road profile');
hold off
%
subplot(4,1,[3 4])
plot(S, Slope, 'b');
hold on
for k1=1:NoPoints
     plot(Points(k1), Slope(Points(k1)/deltas+1), 'ko');
end
latexylabel('$$p$$');
rotateY;
latexxlabel('$$s$$ in m');
axis([0 smax -0.12 0.12]);
yticks([-0.1 0 0.1]);
xticks([0 1000 2000 3000 4000 5000]);
hold off
%


%%  First truck
%   truck with velocity controller
%   input: vref
%   disturbance: d(s_0)
%   outputs:  v_0 = velocity
%             s_0 = position
%   maximum accerleration: amax  ->  maximum velocity amax/c
%
figure(2)
%   Truck model
kP0=0.5;      % parameters of the velocity controller
kI0=0.12;     % for external positive velocity dynamics
A0=[-c 0 0; 1 0 0; 1 0 0];
B0=[1; 0; 0];       % input = acceleration u_0
E0=[-g; 0; 0];      % disturbance = slope of road: p_0
Bw=[0; 0; -1];      % command input = vref
Cu=[-kP0 0 -kI0];   % controller output: u_0
Cv=[1 0 0];         % velocity  v_0
Cs=[0 1 0];         % position s_0
n0=3;
x0=[v0; s0; -c/kI0*v0];
vref=vmax;
%   discrete-time version
FirstTruck=ss(A0, [B0 E0 Bw], Cu, 0);
FirstTruckd=c2d(FirstTruck,Ts);
A0d=FirstTruckd.A;
Bd=FirstTruckd.B;
B0d=Bd(:,1);
E0d=Bd(:,2);
Bwd=Bd(:,3);
%  
%   Behaviour of the first truck on the considered road
Tend0=250;
Time0=0:Ts:Tend0;
kend0=length(Time0);
X0=[];
S0=[];
P0=[];
V0=[];
U0=[];
X0(1,:)=x0';
S0(1,1)=Cs*x0;
P0(1,1)=roadprofile(S0(1,1));
V0(1,1)=Cv*x0;
for k1=2:kend0
    xold=X0(k1-1,:)';     % as column vector
    u0tilde=Cu*xold+kP0*vref;
    if u0tilde>amax
        u0tilde=amax;
    end
    U0(k1-1,1)=u0tilde;
    xnew=A0d*xold+B0d*U0(k1-1,1)+E0d*P0(k1-1,1)+Bwd*vref;
    if u0tilde==amax
        xnew(3)=xold(3);   % anti-windup: integrator is not changed
    end
    if xnew(1,1)>vmaxplus      % reduce the control input if the 
                           % velocity exceeds the tolerable limit
       deltaU=(vmaxplus-xnew(1,1))/B0d(1,1);  % correction term
       U0(k1-1,1)=U0(k1-1,1)+deltaU;      % corrected control input
       xnew=A0d*xold+B0d*U0(k1-1,1)+E0d*P0(k1-1,1)+Bwd*vref;  
                                          % repeat to determine xnew
    end
    X0(k1,:)=xnew';        %  store as row vector
    V0(k1,1)=Cv*xnew;
    S0(k1,1)=Cs*xnew;
    P0(k1,1)=roadprofile(S0(k1,1));
end
u0tilde=Cu*xnew+kP0*vref;
if u0tilde>amax
   u0tilde=amax;
end
U0(k1,1)=u0tilde;
%  figures
%
subplot(6,1,[1 2])
plot(Time0, S0, 'b');
latexylabel('$$s_0$$ in m');
rotateY;
xleer;
axis([0 Tend0 0 5500]);
yticks([0 2500 5000]);
latextitle('$${\tt TruckSelforg:}$$ Behaviour of Truck~0');
%
subplot(6,1,[3 4]);
plot(Time0, V0, 'b');
hold on
for k1=1:NoPoints
    [x1,i1]=min(abs(S0-Points(k1)));
    plot(i1*Ts, V0(i1), 'bo');
end
latexylabel('$$v_0$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
xleer;
axis([0 Tend0 15 27]);
yticks([15 20 25 30]);
hold off
%
subplot(6,1,[5 6]);
plot(Time0, P0, 'b');
hold on
for k1=1:NoPoints
    [x1,i1]=min(abs(S0-Points(k1)));
    plot(i1*Ts, P0(i1), 'bo');
end
latexylabel('$$p_0 \quad $$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 -0.12 0.12]);
yticks([-0.1 0 0.1]);
xticks([0 50 100 150 200 250]);
hold off



%%  Reconstruction of the slope for the first truck
%   Observer for the unknown input p0
%   assumption: p0 is a ramp function
%   applied to the input U0 and the output V0
%   this observer is not used later, but evaluated here for comparison
%   with the observer in figure(4)
%
figure(3)
%  model used for slope reconstruction
AB0=[0 1 0; 0 0 0; -g  0 -c];
BB0=[0; 0; 1];
CB0=[0 0 1];
ObsPoles=[-c -1.1*c -1.3*c]*3.5;  % depending on the vehicle pole -c
lt=place(AB0', CB0', ObsPoles);
ABbar=AB0-lt'*CB0;
%  slope observer  (ABbar, BB0, CB0)
SlopeObs0=ss(ABbar, [BB0 lt'], eye(3,3), 0);
xB0=[0; 0; vref];
X0hat=lsim(SlopeObs0, [U0 V0], Time0, xB0);
%
subplot(4,1,[1 2])
plot(Time0, X0hat(:,1),'b', Time0, P0, 'b--');
latexylabel('$$\hat{p}_0, p_0 \quad$$');
rotateY;
xleer;
axis([0 Tend0 -0.12 0.12]);
yticks([-0.1 0 0.1]);
latextitle('$${\tt TruckSelforg:}$$ Reconstruction of $d_0$');
%
subplot(4,1,[3 4])
plot(Time0, X0hat(:,3),'b', Time0, V0, 'b--');
latexylabel('$$\hat{v}_0, v_0$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 15 27]);
yticks([15 20 25 30]);
xticks([0 50 100 150 200 250]);



%%  Reconstruction of the slope for the first truck
%   Observer for the unknown input p0
%   assumption: p0 is constant
%   applied to the input U0 and the output V0
%
figure(4)
%  model used for slope reconstruction
AB0=[0 0; -g -c];
BB0=[0; 1];
CB0=[0 1];
ObsPoles=[-c -1.1*c]*5;  % depending on the vehicle pole -c
lt=place(AB0', CB0', ObsPoles);
ABbar=AB0-lt'*CB0;
%  slope observer  (ABbar, BB0, CB0)
SlopeObs0=ss(ABbar, [BB0 lt'], eye(2,2), 0);
xB0=[0; vref];
X0hat=lsim(SlopeObs0, [U0 V0], Time0, xB0);
%
subplot(4,1,[1 2])
plot(Time0, X0hat(:,1),'b', Time0, P0, 'b--');
latexylabel('$$\hat{p}_0, p_0 \quad$$');
rotateY;
xleer;
axis([0 Tend0 -0.12 0.12]);
yticks([-0.1 0 0.1]);
latextitle('$${\tt TruckSelforg:}$$ Reconstruction of $d_0$');
%
subplot(4,1,[3 4])
plot(Time0, X0hat(:,2),'b', Time0, V0, 'b--');
latexylabel('$$\hat{v}_0, v_0$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 15 27]);
yticks([15 20 25 30]);
xticks([0 50 100 150 200 250]);


%%  Follower trucks with ACC
%    linear model to evaluate the controller parameters
%
figure(5)
kPi=-1.5;      % parameters of the velocity controller
kDi=-1.5/2;     % for external positive velocity dynamics
betatilde=beta+c/kPi;
ATruck=[(-c+betatilde*kPi+kDi)/(1-betatilde*kDi)  -kPi/(1-betatilde*kDi);
               -1                                     0                 ];
ETruck=[-g/(1-betatilde*kDi); 0];     % input: d_i
BTruck=[-kDi/(1-betatilde*kDi); 1];   % input: v_{i-1}
CTruck=eye(2,2);     % outputs:  v_i, dtilde_i
CTruckv=[1 0];       % output: v_i
%  external positive?
TruckContr=ss(ATruck, BTruck, CTruckv, 0);
[Yi, Timei]=impulse(TruckContr);
subplot(2,1,1)
plot(Timei, Yi, 'b', [0 max(Timei)], [0, 0], 'k:');
axis([0 max(Timei), -0.02, 0.4]);
latextitle('$${\tt TruckSelforg:}$$ Test of external positivity');
latexylabel('$$\bar{g}_i \;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 0.2 0.4]);
xticks([0 5 10]);
hold off;

%%  Follower trucks with ACC
%   nonlinear model used to follow the first truck
%
figure(6)
a1max=amax*0.95;
%   Truck model: controller parameters as before
Ai=[-c 0; -1 0];    % state: [v_i, dtilde_i]
Bi=[1; 0];          % input = acceleration u_i
Ei=[-g; 0];         % disturbance = slope of road: p_i
Bvi=[0; 1];         % velocity of predecessor: v_{i-1}
Cui=[(betatilde*kPi-betatilde*kDi*c+kDi)/(1-betatilde*kDi)  -kPi/(1-betatilde*kDi)];
                % controller output: u_i
Cvi=[1 0];         % velocity  v_i
Cdi=[0 1];         % vehicle distance dtilde_i
ni=2;
%   initial state of Truck 1
v10=v0;
dtilde10=beta*v0;
x10=[v10; dtilde10];
%   discrete-time version
FirstFollower=ss(Ai, [Bi Ei Bvi], Cui, 0);
FirstFollowerd=c2d(FirstFollower,Ts);
Aid=FirstFollowerd.A;
Bd=FirstFollowerd.B;
Bid=Bd(:,1);
Eid=Bd(:,2);
Bvid=Bd(:,3);
%  
%   Behaviour of the first follower 
%   for given behaviour of the first truck: V0, S0
X1=[];
D1=[];
S1=[];
P1=[];
V1=[];
U1=[];
X1(1,:)=x10';
D1(1,1)=Cdi*x10;
S1(1,1)=S0(1,1)-trucklength-alpha-D1(1,1);
P1(1,1)=roadprofile(S1(1,1));
V1(1,1)=Cvi*X1(1,:)';
for k1=2:kend0
    xold=X1(k1-1,:)';     % as column vector
    u1tilde=Cui*xold+-betatilde*kDi*g/(1-betatilde*kDi)*P1(k1-1,1)...
            -kDi/(1-betatilde*kDi)*V0(k1-1,1);
    if u1tilde>a1max
        u1tilde=a1max;
    end
    U1(k1-1,1)=u1tilde;
    xnew=Aid*xold+Bid*U1(k1-1)+Eid*P1(k1-1)+Bvid*V0(k1-1,1);
    if xnew(1,1)>vmaxplus      % reduce the control input if the 
                           % velocity exceeds the limit
    deltaU=(vmaxplus-xnew(1,1))/Bid(1,1);  % correction term
    U1(k1-1,1)=U1(k1-1,1)+deltaU;      % corrected control input
    xnew=Aid*xold+Bid*U1(k1-1)+Eid*P1(k1-1)+Bvid*V0(k1-1,1); 
                                          % repeat to determine xnew
    end
    X1(k1,:)=xnew';        %  store as row vector
    V1(k1,1)=Cvi*xnew;
    D1(k1,1)=Cdi*xnew;
    S1(k1,1)=S0(k1,1)-trucklength-alpha-D1(k1,1);
    P1(k1,1)=roadprofile(S1(k1,1));
end
u1tilde=Cui*xnew+-betatilde*kDi*g/(1-betatilde*kDi)*P1(k1,1)...
        -kDi/(1-betatilde*kDi)*V0(k1,1);
if u1tilde>a1max
   u1tilde=a1max;
end
U1(k1,1)=u1tilde;
%  figures
%
subplot(6,1,[1 2])
plot(Time0, S1, 'b', Time0, S0, 'b--');
latexylabel('$$s_1, s_0$$ in m');
rotateY;
xleer;
axis([0 Tend0 0 5500]);
yticks([0 2500 5000]);
latextitle('$${\tt TruckSelforg:}$$ Behaviour of Truck~1');
%
subplot(6,1,[3 4]);
plot(Time0, V1, 'b');
hold on
for k1=1:NoPoints
    [x1,i1]=min(abs(S1-Points(k1)));
    plot(i1*Ts, V1(i1), 'bo');
end
latexylabel('$$v_1$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
xleer;
axis([0 Tend0 12 28]);
yticks([15 20 25 30]);
hold off
%
subplot(6,1,[5 6]);
plot(Time0, P1, 'b');
hold on
for k1=1:NoPoints
    [x1,i1]=min(abs(S1-Points(k1)));
    plot(i1*Ts, P1(i1), 'bo');
end
latexylabel('$$p_1 \quad $$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 -0.12 0.12]);
yticks([-0.1 0 0.1]);
xticks([0 50 100 150 200 250]);
hold off
%
figure(7)
subplot(3,1,[1 2])
plot(Time0, D1, 'b', Time0, V1*beta, 'b--');
hold on
for k1=1:NoPoints
    [x1,i1]=min(abs(S1-Points(k1)));
    plot(i1*Ts, D1(i1), 'bo');
end
latextitle('$${\tt TruckSelforg:}$$ Distance between Trucks 0 and 1');
latexylabel('$$d_1$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 0 350]);
yticks([0 100 200 300]);
xticks([0 50 100 150 200 250]);
latextext(150, 100, '$$d_{1 {\rm ref}}$$');
hold off

%%  Reconstruction of the slope for the first follower
%   Observer for the unknown input p1
%   assumption: p1 is constant
%   applied to the input U1 and the output V1
%
figure(8)
%  model used for slope reconstruction
ABi=[0 0; -g -c];
BBi=[0; 1];
CBi=[0 1];
ObsPoles=[-c -1.1*c]*5;  % depending on the vehicle pole -c
lti=place(ABi', CBi', ObsPoles);
ABibar=ABi-lti'*CBi;
%  slope observer  (ABbar, BB0, CB0)
SlopeObsi=ss(ABibar, [BBi lti'], eye(2,2), 0);
xBi0=[0; vref];
X1hat=lsim(SlopeObsi, [U1 V1], Time0, xBi0);
%
subplot(4,1,[1 2])
plot(Time0, X1hat(:,1),'b', Time0, P1, 'b--');
latexylabel('$$\hat{p}_1, p_1 \quad$$');
rotateY;
xleer;
axis([0 Tend0 -0.12 0.12]);
yticks([-0.1 0 0.1]);
latextitle('$${\tt TruckSelforg:}$$ Reconstruction of $p_1$');
%
subplot(4,1,[3 4])
plot(Time0, X1hat(:,2),'b', Time0, V1, 'b--');
latexylabel('$$\hat{v}_1, v_1$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 12 27]);
yticks([15 20 25 30]);
xticks([0 50 100 150 200 250]);

%%   Combination of Truck 0 and Truck 1 in one unique model
%   and determination of the behaviour on the given road profile
%
figure(9)
%  model of two trucks
AP1=[A0d zeros(n0,ni);
     Bvid*Cv   Aid];
BP1=[B0d zeros(n0,1);
     zeros(ni,1) Bid];
EP1=[E0d zeros(n0,1);
     zeros(ni,1) Eid];
BwP1=[Bwd; zeros(ni,1)];
CPd1=[zeros(1,n0) Cdi];    % output: dtilde_1
CPs1=[Cs zeros(1,ni)];     % output: s_0
CPv1=[Cv zeros(1,ni); 
      zeros(1,n0) Cvi];    % outputs: [v_0; v_1]
n1=n0+ni;
xP01=[x0; x10];
vref=vmax;
%   Behaviour of the first two trucks
XP1=[];   % [x0' x1']  as row vector
DP1=[];   % dtilde_1 scalar
SP1=[];   % {s0; s1]
PP1=[];   % [p0; p1]
VP1=[];   % {v0; v1]
UP1=[];   % [u0; u1]
XP1(1,:)=xP01';
DP1(1,1)=CPd1*XP1(1,:)';
SP1(1,:)=[CPs1*XP1(1,:)' CPs1*XP1(1,:)'-trucklength-alpha-DP1(1,1)];
PP1(1,:)=[roadprofile(SP1(1,1)) roadprofile(SP1(1,2))];
VP1(1,:)=(CPv1*XP1(1,:)')';
for k1=2:kend0
    xold=XP1(k1-1,:)';     % as column vector
    %  control input for Truck 0
    u0tilde=Cu*xold(1:n0)+kP0*vref;
    if u0tilde>amax
        u0tilde=amax;
    end
    UP1(k1-1,1)=u0tilde;
    %  control input for Truck 1
    u1tilde=Cui*xold(n0+1:n0+ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP1(k1-1,2)...
            -kDi/(1-betatilde*kDi)*VP1(k1-1,2);
    if u1tilde>a1max
        u1tilde=a1max;
    end
    UP1(k1-1,2)=u1tilde;
    %  new overall state
    xnew=AP1*xold+BP1*UP1(k1-1,:)'+EP1*PP1(k1-1,:)'+BwP1*vref;
    newU=0;                % indicator for corrected input
    if xnew(1,1)>vmaxplus      % reduce the control input if the 
                           % velocity exceeds the tolerance limit
       deltaU=(vmaxplus-xnew(1,1))/BP1(1,1);  % correction term
       UP1(k1-1,1)=UP1(k1-1,1)+deltaU;      % corrected control input
       newU=1;
    end
    if xnew(n0+1,1)>vmaxplus      % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP1(n0+1,2);  % correction term
        UP1(k1-1,2)=UP1(k1-1,2)+deltaU;      % corrected control input
        newU=1;
    end
    if newU==1
       xnew=AP1*xold+BP1*UP1(k1-1,:)'+EP1*PP1(k1-1,:)'+BwP1*vref;
                                          % repeat to determine xnew
    end
    if u0tilde==amax
       xnew(3,1)=xold(3,1);   % anti-windup: integrator is not changed
    end
    XP1(k1,:)=xnew';        %  store as row vector
    DP1(k1,1)=CPd1*XP1(k1,:)';
    SP1(k1,:)=[CPs1*XP1(k1,:)'...
               CPs1*XP1(k1,:)'-trucklength-alpha-DP1(k1,1)];
    PP1(k1,:)=[roadprofile(SP1(k1,1)) roadprofile(SP1(k1,2))];
    VP1(k1,:)=(CPv1*XP1(k1,:)')';
end
u0tilde=Cu*xnew(1:n0)+kP0*vref;
if u0tilde>amax
   u0tilde=amax;
end
UP1(k1,1)=u0tilde;
u1tilde=Cui*xnew(n0+1:n0+ni)...
        -betatilde*kDi*g/(1-betatilde*kDi)*PP1(k1-1,2)...
        -kDi/(1-betatilde*kDi)*VP1(k1-1,2);
if u1tilde>a1max
   u1tilde=a1max;
end
UP1(k1,2)=u1tilde;
%  figures
%
subplot(6,1,[1 2])
plot(Time0, SP1(:,1), 'b', Time0, SP1(:,2), 'b--');
latexylabel('$$s_0, s_1$$ in m');
rotateY;
xleer;
axis([0 Tend0 0 5500]);
yticks([0 2500 5000]);
latextitle('$${\tt TruckSelforg:}$$ Behaviour of Trucks~0 and 1');
%
subplot(6,1,[3 4]);
plot(Time0, VP1(:,1), 'b', Time0, VP1(:,2), 'b--');
latexylabel('$$v_0, v_1$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
xleer;
axis([0 Tend0 12 27]);
yticks([15 20 25 30]);
%
subplot(6,1,[5 6]);
plot(Time0, PP1(:,1), 'b', Time0, PP1(:,2), 'b--');
latexylabel('$$p_0, p_1 \;\; $$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 -0.12 0.12]);
yticks([-0.1 0 0.1]);
xticks([0 50 100 150 200 250]);
%
figure(10)
subplot(3,1,[1 2])
plot(Time0, DP1, 'b', Time0, VP1(:,2)*beta, 'b--');
latextitle('$${\tt TruckSelforg:}$$ Distance between Trucks 0 and 1');
latexylabel('$$d_1$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 0 350]);
yticks([0 100 200 300]);
xticks([0 50 100 150 200 250]);
latextext(150, 100, '$$d_{1 {\rm ref}}$$');


%%   Combination of Truck 0 and Truck 1 in one unique model
%   extension with reconstruction algorithm for the slope
%   and determination of the behaviour on the given road profile
%
figure(11)
%   Reconstruction algorithm for Truck 0
SlopeObs0d=c2d(SlopeObs0,Ts);
AR0=SlopeObs0d.A;
BR0=SlopeObs0d.B;
CR0=SlopeObs0d.C;
DR0=SlopeObs0d.D;
xR0=[0; vref];
%   Reconstruction algorithm for Truck 1
SlopeObsid=c2d(SlopeObsi,Ts);
ARi=SlopeObsid.A;
BRi=SlopeObsid.B;
CRi=SlopeObsid.C;
DRi=SlopeObsid.D;
xRi0=[0; vref];
%   Behaviour of the first two trucks
XP1=[];   % [x0' x1']  as row vector
DP1=[];   % dtilde_1 scalar
SP1=[];   % {s0; s1]
PP1=[];   % [p0; p1]
VP1=[];   % {v0; v1]
UP1=[];   % [u0; u1]
XP1(1,:)=xP01';
DP1(1,1)=CPd1*XP1(1,:)';
SP1(1,:)=[CPs1*XP1(1,:)' CPs1*XP1(1,:)'-trucklength-alpha-DP1(1,1)];
PP1(1,:)=[roadprofile(SP1(1,1)) roadprofile(SP1(1,2))];
VP1(1,:)=(CPv1*XP1(1,:)')';
PP1hat(1,:)=[xR0(1,1) xRi0(1,1)];
xR0old=xR0;
xR1old=xRi0;
for k1=2:kend0
    xold=XP1(k1-1,:)';     % as column vector
    %  control input for Truck 0
    u0tilde=Cu*xold(1:n0)+kP0*vref;
    if u0tilde>amax
        u0tilde=amax;
    end
    UP1(k1-1,1)=u0tilde;
    %  control input for Truck 1
    u1tilde=Cui*xold(n0+1:n0+ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP1(k1-1,2)...
            -kDi/(1-betatilde*kDi)*VP1(k1-1,1);
    if u1tilde>a1max
        u1tilde=a1max;
    end
    UP1(k1-1,2)=u1tilde;
    %  new overall state
    xnew=AP1*xold+BP1*UP1(k1-1,:)'+EP1*PP1(k1-1,:)'+BwP1*vref;
    newU=0;                % indicator for corrected input
    if xnew(1,1)>vmaxplus      % reduce the control input if the 
                           % velocity exceeds the limit
       deltaU=(vmaxplus-xnew(1,1))/BP1(1,1);  % correction term
       UP1(k1-1,1)=UP1(k1-1,1)+deltaU;      % corrected control input
       newU=1;
    end
    if xnew(n0+1,1)>vmaxplus      % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP1(n0+1,2);  % correction term
        UP1(k1-1,2)=UP1(k1-1,2)+deltaU;      % corrected control input
        newU=1;
    end
    if newU==1
       xnew=AP1*xold+BP1*UP1(k1-1,:)'+EP1*PP1(k1-1,:)'+BwP1*vref;
                                          % repeat to determine xnew
    end
    if u0tilde==amax
       xnew(3,1)=xold(3,1);   % anti-windup: integrator is not changed
    end
    XP1(k1,:)=xnew';        %  store as row vector
    DP1(k1,1)=CPd1*XP1(k1,:)';
    SP1(k1,:)=[CPs1*XP1(k1,:)'...
               CPs1*XP1(k1,:)'-trucklength-alpha-DP1(k1,1)];
    PP1(k1,:)=[roadprofile(SP1(k1,1)) roadprofile(SP1(k1,2))];
    VP1(k1,:)=(CPv1*XP1(k1,:)')';
    %  Reconstruction of the slope
    %  Truck 0
    xR0new=AR0*xR0old+BR0*[UP1(k1-1,1); VP1(k1,1)];
    PP1hat(k1,1)=xR0new(1,1);
    xR0old=xR0new;
    %  Truck 1
    xR1new=ARi*xR1old+BRi*[UP1(k1-1,2); VP1(k1,2)];
    PP1hat(k1,2)=xR1new(1,1);
    xR1old=xR1new;    
end
u0tilde=Cu*xnew(1:n0)+kP0*vref;
if u0tilde>amax
   u0tilde=amax;
end
UP1(k1,1)=u0tilde;
u1tilde=Cui*xnew(n0+1:n0+ni)...
        -betatilde*kDi*g/(1-betatilde*kDi)*PP1(k1-1,2)...
        -kDi/(1-betatilde*kDi)*VP1(k1-1,2);
if u1tilde>a1max
   u1tilde=a1max;
end
UP1(k1,2)=u1tilde;
%  figures
%
subplot(6,1,[1 2])
plot(Time0, PP1hat(:,1), 'b', Time0, PP1(:,1), 'b--');
latexylabel('$$\hat{p}_0, p_0 \;\;$$');
rotateY;
xleer;
axis([0 Tend0 -0.12 0.12]);
yticks([-0.1 0 0.1]);
latextitle('$${\tt TruckSelforg:}$$ Slope reconstruction');
%
subplot(6,1,[3 4]);
plot(Time0, PP1hat(:,2), 'b', Time0, PP1(:,2), 'b--');
latexylabel('$$\hat{p}_1, p_1 \;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 -0.12 0.12]);
yticks([-0.1 0 0.1]);
xticks([0 50 100 150 200 250]);


%%   Combination of Truck 0 and Truck 1 in one unique model
%   Determination of the reference velocity in dependence upon the slope
%   and determination of the behaviour on the given road profile
%
figure(12)
%   Behaviour of the first two trucks
XP1=[];   % [x0' x1']  as row vector
DP1=[];   % dtilde_1 scalar
SP1=[];   % {s0; s1]
PP1=[];   % [p0; p1]
VP1=[];   % {v0; v1]
UP1=[];   % [u0; u1]
Vref=[];  % vref(k1)
XP1(1,:)=xP01';
DP1(1,1)=CPd1*XP1(1,:)';
SP1(1,:)=[CPs1*XP1(1,:)' CPs1*XP1(1,:)'-trucklength-alpha-DP1(1,1)];
PP1(1,:)=[roadprofile(SP1(1,1)) roadprofile(SP1(1,2))];
VP1(1,:)=(CPv1*XP1(1,:)')';
PP1hat(1,:)=[xR0(1,1) xRi0(1,1)];
xR0old=xR0;
xR1old=xRi0;
v0hat=amax/c-g/c*PP1hat(1,1);
v1hat=a1max/c-g/c*PP1hat(1,2);
vref=min([v0hat, v1hat, vmax]);
Vref(1,1)=vref;
for k1=2:kend0
    xold=XP1(k1-1,:)';     % as column vector
    %  control input for Truck 0
    u0tilde=Cu*xold(1:n0)+kP0*vref;
    if u0tilde>amax
        u0tilde=amax;
    end
    UP1(k1-1,1)=u0tilde;
    %  control input for Truck 1
    u1tilde=Cui*xold(n0+1:n0+ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP1(k1-1,2)...
            -kDi/(1-betatilde*kDi)*VP1(k1-1,1);
    if u1tilde>a1max
        u1tilde=a1max;
    end
    UP1(k1-1,2)=u1tilde;
    %  new overall state
    xnew=AP1*xold+BP1*UP1(k1-1,:)'+EP1*PP1(k1-1,:)'+BwP1*vref;
    newU=0;                % indicator for corrected input
    if xnew(1,1)>vmaxplus      % reduce the control input if the 
                           % velocity exceeds the limit
       deltaU=(vmaxplus-xnew(1,1))/BP1(1,1);  % correction term
       UP1(k1-1,1)=UP1(k1-1,1)+deltaU;      % corrected control input
       newU=1;
    end
    if xnew(n0+1,1)>vmaxplus      % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP1(n0+1,2);  % correction term
        UP1(k1-1,2)=UP1(k1-1,2)+deltaU;      % corrected control input
        newU=1;
    end
    if newU==1
       xnew=AP1*xold+BP1*UP1(k1-1,:)'+EP1*PP1(k1-1,:)'+BwP1*vref;
                                          % repeat to determine xnew
    end
    if u0tilde==amax
       xnew(3,1)=xold(3,1);   % anti-windup: integrator is not changed
    end
    XP1(k1,:)=xnew';        %  store as row vector
    DP1(k1,1)=CPd1*XP1(k1,:)';
    SP1(k1,:)=[CPs1*XP1(k1,:)'...
               CPs1*XP1(k1,:)'-trucklength-alpha-DP1(k1,1)];
    PP1(k1,:)=[roadprofile(SP1(k1,1)) roadprofile(SP1(k1,2))];
    VP1(k1,:)=CPv1*XP1(k1,:)';
    %  Reconstruction of the slope
    %  Truck 0
    xR0new=AR0*xR0old+BR0*[UP1(k1-1,1); VP1(k1,1)];
    PP1hat(k1,1)=xR0new(1,1);
    xR0old=xR0new;
    %  Truck 1
    xR1new=ARi*xR1old+BRi*[UP1(k1-1,2); VP1(k1,2)];
    PP1hat(k1,2)=xR1new(1,1);
    xR1old=xR1new;  
    %  Determination of the reference velocity
    v0hat=0.9*(amax/c-g/c*PP1hat(k1,1));
    v1hat=0.9*(a1max/c-g/c*PP1hat(k1,2));
    vref=min([v0hat, v1hat, vmax]);
    Vref(k1,1)=vref;
end
u0tilde=Cu*xnew(1:n0)+kP0*vref;
if u0tilde>amax
   u0tilde=amax;
end
UP1(k1,1)=u0tilde;
u1tilde=Cui*xnew(n0+1:n0+ni)...
        -betatilde*kDi*g/(1-betatilde*kDi)*PP1(k1-1,2)...
        -kDi/(1-betatilde*kDi)*VP1(k1-1,1);
if u1tilde>a1max
   u1tilde=a1max;
end
UP1(k1,2)=u1tilde;
%  figures
%
subplot(6,1,[1 2])
plot(Time0, Vref, 'b');
hold on
%plot(Time0, VP1(:,1), 'b:');
plot(Time0, VP1(:,2), 'b--');
latexylabel('$$v_{\rm ref}, v_1$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
xleer;
latextitle('$${\tt TruckSelforg:}$$ Selforganised platoon with 2 trucks');
yticks([10 20 30]);
axis([0 Tend0 10 30]);
hold off
%
subplot(6,1,[3 4])
plot(Time0, DP1, 'b', Time0, VP1(:,2)*beta, 'b--');
latexylabel('$$d_1, d_{1 {\rm ref}}$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 0 60]);
yticks([0 30 60]);
xticks([0 50 100 150 200 250]);



%%   Platoon with leader and 5 followers
%   Determination of the reference velocity in dependence upon the slope
%   and determination of the behaviour on the given road profile
%
figure(13)
N1=5;    % number of followers
a1max=amax*0.93;
a2max=amax;
a3max=amax;
a4max=a1max;
a5max=amax*0.95;
%  Model of six trucks
AP5=[A0d zeros(n0,N1*ni);
     Bvid*Cv Aid zeros(ni,(N1-1)*ni);
     zeros(ni,n0) Bvid*Cvi  Aid zeros(ni,(N1-2)*ni);
     zeros(ni,n0) zeros(ni,ni) Bvid*Cvi  Aid zeros(ni,(N1-3)*ni);
     zeros(ni,n0) zeros(ni,2*ni) Bvid*Cvi  Aid zeros(ni,(N1-4)*ni);
     zeros(ni,n0) zeros(ni,3*ni) Bvid*Cvi  Aid];
BP5=[B0d zeros(n0,N1);
     zeros(ni,1) Bid zeros(ni,(N1-1));
     zeros(ni,2) Bid zeros(ni,(N1-2));
     zeros(ni,3) Bid zeros(ni,(N1-3));
     zeros(ni,4) Bid zeros(ni,(N1-4));
     zeros(ni,5) Bid ];
EP5=[E0d zeros(n0,N1);
     zeros(ni,1) Eid zeros(ni,(N1-1));
     zeros(ni,2) Eid zeros(ni,(N1-2));
     zeros(ni,3) Eid zeros(ni,(N1-3));
     zeros(ni,4) Eid zeros(ni,(N1-4));
     zeros(ni,5) Eid ];
BwP5=[Bwd; zeros(N1*ni,1)];
                  % output: [dtilde_1 dtilde_2 dtilde_3 dtilde_4 dtilde_5]
CPd5=[zeros(1,n0) Cdi zeros(1,(N1-1)*ni);
      zeros(1,n0+ni) Cdi zeros(1,(N1-2)*ni);
      zeros(1,n0+2*ni) Cdi zeros(1,(N1-3)*ni);
      zeros(1,n0+3*ni) Cdi zeros(1,(N1-4)*ni);
      zeros(1,n0+4*ni) Cdi zeros(1,(N1-5)*ni)];    
CPs5=[Cs zeros(1,N1*ni)];     % output: s_0
                  % output: [v_0 v_1 v_2 v_3 v_4 v_5]
CPv5=[Cv zeros(1,N1*ni); 
      zeros(1,n0) Cvi zeros(1,(N1-1)*ni);
      zeros(1,n0+ni) Cvi zeros(1,(N1-2)*ni);
      zeros(1,n0+2*ni) Cvi zeros(1,(N1-3)*ni);
      zeros(1,n0+3*ni) Cvi zeros(1,(N1-4)*ni);
      zeros(1,n0+4*ni) Cvi zeros(1,(N1-5)*ni)];   
v0=vmax;
dtilde0=beta*v0;
xi0=[v0; dtilde0];
xP05=[x0; xi0; xi0; xi0; xi0; xi0];
vref=vmax;
%   Behaviour of six trucks
XP5=[];   % [x0' x1' x2' x3' x4' x5']  as row vector
DP5=[];   % [dtilde_1 dtilde_2 dtilde_3 dtilde_4 dtilde_6] 
SP5=[];   % {s0 s1 s2 s3 s4 s5]
PP5=[];   % [p0 p1 p2 p3 p4 p5]
PP5hat=[];
VP5=[];   % {v0 v1 v2 v3 v4 v5]
UP5=[];   % [u0 u1 u2 u3 u4 u5]
Vref=[];  % vref
Truck=[]; % number of truck that determines the velocity
XP5(1,:)=xP05';
DP5(1,:)=CPd5*XP5(1,:)';
SP5(1,:)=[CPs5*XP5(1,:)' zeros(1,N1)];
for k2=1:N1
    SP5(1,k2+1)=SP5(1,k2)-trucklength-alpha-DP5(1,k2);
end
PP5(1,1)=roadprofile(SP5(1,1));
for k2=1:N1
    PP5(1,k2+1)=roadprofile(SP5(1,k2+1));
end
VP5(1,:)=CPv5*XP5(1,:)';
PP5hat(1,:)=[xR0(1,1) xRi0(1,1) xRi0(1,1) xRi0(1,1) xRi0(1,1) xRi0(1,1)];
xR0old=xR0;
xR1old=xRi0;
xR2old=xRi0;
xR3old=xRi0;
xR4old=xRi0;
xR5old=xRi0;
v0hat=amax/c-g/c*PP5hat(1,1);
v1hat=a1max/c-g/c*PP5hat(1,2);
v2hat=a2max/c-g/c*PP5hat(1,3);
v3hat=a3max/c-g/c*PP5hat(1,4);
v4hat=a4max/c-g/c*PP5hat(1,5);
v5hat=a5max/c-g/c*PP5hat(1,6);
vref=min([v0hat, v1hat, v2hat, v3hat, v4hat, v5hat, vmax]);
Vref(1,1)=vref;
Truck(1,1)=7;   % velocity determined by vmax
for k1=2:kend0
    xold=XP5(k1-1,:)';     % as column vector
    %  control input for Truck 0
    u0tilde=Cu*xold(1:n0)+kP0*Vref(k1-1,1);
    if u0tilde>amax
        u0tilde=amax;
    end
    UP5(k1-1,1)=u0tilde;
    %  control input for Truck 1
    u1tilde=Cui*xold(n0+1:n0+ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,2)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,1);
    if u1tilde>a1max
        u1tilde=a1max;
    end
    UP5(k1-1,2)=u1tilde;
    %  control input for Truck 2
    u2tilde=Cui*xold(n0+ni+1:n0+2*ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,3)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,2);
    if u2tilde>a2max
        u2tilde=a2max;
    end
    UP5(k1-1,3)=u2tilde;
    %  control input for Truck 3
    u3tilde=Cui*xold(n0+2*ni+1:n0+3*ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,4)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,3);
    if u3tilde>a3max
        u3tilde=a3max;
    end
    UP5(k1-1,4)=u3tilde;
    %  control input for Truck 4
    u4tilde=Cui*xold(n0+3*ni+1:n0+4*ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,5)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,4);
    if u4tilde>a4max
        u4tilde=a4max;
    end
    UP5(k1-1,5)=u4tilde;
    %  control input for Truck 5
    u5tilde=Cui*xold(n0+4*ni+1:n0+5*ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,6)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,5);
    if u5tilde>a5max
        u5tilde=a5max;
    end
    UP5(k1-1,6)=u5tilde;
    %  new overall state
    xnew=AP5*xold+BP5*UP5(k1-1,:)'+EP5*PP5(k1-1,:)'+BwP5*Vref(k1-1,1);
    newU=0;                % indicator for corrected input
    if xnew(1,1)>vmaxplus  % reduce the control input if the 
                           % velocity exceeds the limit
       deltaU=(vmaxplus-xnew(1,1))/BP5(1,1);  % correction term
       UP5(k1-1,1)=UP5(k1-1,1)+deltaU;      % corrected control input
       newU=1;
    end
    if xnew(n0+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+1,2);  % correction term
        UP5(k1-1,2)=UP5(k1-1,2)+deltaU;      % corrected control input
        newU=1;
    end
    if xnew(n0+ni+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+ni+1,3);  % correction term
        UP5(k1-1,3)=UP5(k1-1,3)+deltaU;      % corrected control input
        newU=1;
    end
    if xnew(n0+2*ni+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+2*ni+1,4);  % correction term
        UP5(k1-1,4)=UP5(k1-1,4)+deltaU;      % corrected control input
        newU=1;
    end
    if xnew(n0+3*ni+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+3*ni+1,5);  % correction term
        UP5(k1-1,5)=UP5(k1-1,5)+deltaU;      % corrected control input
        newU=1;
    end
    if xnew(n0+4*ni+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+4*ni+1,6);  % correction term
        UP5(k1-1,6)=UP5(k1-1,6)+deltaU;      % corrected control input
        newU=1;
    end
    if newU==1
       xnew=AP5*xold+BP5*UP5(k1-1,:)'+EP5*PP5(k1-1,:)'+BwP5*Vref(k1-1,1);
                                          % repeat to determine xnew
    end
    if u0tilde==amax
       xnew(3,1)=xold(3,1);   % anti-windup: integrator is not changed
    end
    XP5(k1,:)=xnew';        %  store as row vector
    DP5(k1,:)=CPd5*XP5(k1,:)';
    SP5(k1,:)=[CPs5*XP5(k1,:)' zeros(1,N1)];
    for k2=1:N1
        SP5(k1,k2+1)=SP5(k1,k2)-trucklength-alpha-DP5(k1,k2);
    end
    PP5(k1,:)=[roadprofile(SP5(k1,1)) zeros(1,N1)];
    for k2=1:N1
        PP5(k1,k2+1)=roadprofile(SP5(k1,k2+1));
    end
    VP5(k1,:)=CPv5*XP5(k1,:)';
    %  Reconstruction of the slope
    %  Truck 0
    xR0new=AR0*xR0old+BR0*[UP5(k1-1,1); VP5(k1,1)];
    PP5hat(k1,1)=xR0new(1,1);
    xR0old=xR0new;
    %  Truck 1
    xR1new=ARi*xR1old+BRi*[UP5(k1-1,2); VP5(k1,2)];
    PP5hat(k1,2)=xR1new(1,1);
    xR1old=xR1new;  
    %  Truck 2
    xR2new=ARi*xR2old+BRi*[UP5(k1-1,3); VP5(k1,3)];
    PP5hat(k1,3)=xR2new(1,1);
    xR2old=xR2new;  
    %  Truck 3
    xR3new=ARi*xR3old+BRi*[UP5(k1-1,4); VP5(k1,4)];
    PP5hat(k1,4)=xR3new(1,1);
    xR3old=xR3new;  
    %  Truck 4
    xR4new=ARi*xR4old+BRi*[UP5(k1-1,5); VP5(k1,5)];
    PP5hat(k1,5)=xR4new(1,1);
    xR4old=xR4new;  
    %  Truck 5
    xR5new=ARi*xR5old+BRi*[UP5(k1-1,6); VP5(k1,6)];
    PP5hat(k1,6)=xR5new(1,1);
    xR5old=xR5new;  
    %  Determination of the reference velocity
    v0hat=gamma*(amax/c-g/c*PP5hat(k1,1));
    v1hat=gamma*(a1max/c-g/c*PP5hat(k1,2));
    v2hat=gamma*(a2max/c-g/c*PP5hat(k1,3));
    v3hat=gamma*(a3max/c-g/c*PP5hat(k1,4));
    v4hat=gamma*(a4max/c-g/c*PP5hat(k1,5));
    v5hat=gamma*(a5max/c-g/c*PP5hat(k1,6));
    [vref,truck]=min([v0hat, v1hat, v2hat, v3hat, v4hat, v5hat, vmax]');
    Vref(k1,1)=vref;     % vref
    Truck(k1,1)=truck-1; % index of truck that determines reference velocity
end
%  simplified version to get the last (unimportant) value of U5
UP5(k1,:)=UP5(k1-1,:);
%  figures
%
subplot(5,1,[1 2])
plot(Time0, Vref, 'b');
hold on
plot(Time0, VP5(:,1), 'b--');
plot(Time0, VP5(:,2), 'b--');
plot(Time0, VP5(:,3), 'b--');
plot(Time0, VP5(:,4), 'b--');
plot(Time0, VP5(:,5), 'b--');
plot(Time0, VP5(:,6), 'b--');
latexylabel('$$v_{\rm ref}, v_i$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
xleer;
latextitle('$${\tt TruckSelforg:}$$ Selforganised platoon');
yticks([15 20 25]);
axis([0 Tend0 12 27]);
hold off
subplot(5,1,[3 4])
plot(Time0, Truck, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
hold on
for k1=1:NoPoints
    [x1,i1]=min(abs(SP5(:,1)-Points(k1)));  % time point at which 
                                            % the leader reaches Point(k1)
    plot([i1*Ts i1*Ts], [0 5], 'b--');
end
latexxlabel('$$t$$ in s');
latexylabel('Truck number');
rotateY;
axis([0 Tend0, 0 5]);
yticks([0 1 2 3 4 5]);
hold off
%    Figure for performance comparison (see below)
figure(15)
subplot(4,1,[3 4])
plot(Time0, SP5(:,1:N1), 'b');
latexylabel('$$s_i$$ in m $$\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([180 230 2700 5000]);
yticks([0 3000 4000 5000]);
xticks([0 50 100 180 200 220 240]);
%

%%   Truck distances
figure(14)
subplot(4,1,[1 2])
plot(Time0, DP5(:,1), 'b', Time0, VP5(:,2)*beta, 'b--');
latexylabel('$$d_1, d_{1 {\rm ref}}$$ in m');
rotateY;
xleer;
axis([0 Tend0 20 60]);
yticks([0 30 50]);
%xticks([0 50 100 150 200]);
%
subplot(4,1,[3 4])
plot(Time0, DP5, 'b');
latexylabel('$$d_i$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend0 20 60]);
yticks([0 30 50]);
xticks([0 50 100 150 200 250]);

%%  Comparison of self-organised control and fixed control
%
figure(15)
%  Truck platoon with fixed controllers
%   Behaviour of six trucks
XP5=[];   % [x0' x1' x2' x3' x4' x5']  as row vector
DP5=[];   % [dtilde_1 dtilde_2 dtilde_3 dtilde_4 dtilde_6] 
SP5=[];   % {s0 s1 s2 s3 s4 s5]
PP5=[];   % [p0 p1 p2 p3 p4 p5]
PP5hat=[];
VP5=[];   % {v0 v1 v2 v3 v4 v5]
UP5=[];   % [u0 u1 u2 u3 u4 u5]
Vref=[];  % vref
Truck=[]; % number of truck that determines the velocity
XP5(1,:)=xP05';
DP5(1,:)=CPd5*XP5(1,:)';
SP5(1,:)=[CPs5*XP5(1,:)' zeros(1,N1)];
for k2=1:N1
    SP5(1,k2+1)=SP5(1,k2)-trucklength-alpha-DP5(1,k2);
end
PP5(1,1)=roadprofile(SP5(1,1));
for k2=1:N1
    PP5(1,k2+1)=roadprofile(SP5(1,k2+1));
end
VP5(1,:)=CPv5*XP5(1,:)';
PP5hat(1,:)=[xR0(1,1) xRi0(1,1) xRi0(1,1) xRi0(1,1) xRi0(1,1) xRi0(1,1)];
Vref(1,1)=vmax;
for k1=2:kend0
    xold=XP5(k1-1,:)';     % as column vector
    %  control input for Truck 0
    u0tilde=Cu*xold(1:n0)+kP0*Vref(k1-1,1);
    if u0tilde>amax
        u0tilde=amax;
    end
    UP5(k1-1,1)=u0tilde;
    %  control input for Truck 1
    u1tilde=Cui*xold(n0+1:n0+ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,2)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,1);
    if u1tilde>a1max
        u1tilde=a1max;
    end
    UP5(k1-1,2)=u1tilde;
    %  control input for Truck 2
    u2tilde=Cui*xold(n0+ni+1:n0+2*ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,3)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,2);
    if u2tilde>a2max
        u2tilde=a2max;
    end
    UP5(k1-1,3)=u2tilde;
    %  control input for Truck 3
    u3tilde=Cui*xold(n0+2*ni+1:n0+3*ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,4)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,3);
    if u3tilde>a3max
        u3tilde=a3max;
    end
    UP5(k1-1,4)=u3tilde;
    %  control input for Truck 4
    u4tilde=Cui*xold(n0+3*ni+1:n0+4*ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,5)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,4);
    if u4tilde>a4max
        u4tilde=a4max;
    end
    UP5(k1-1,5)=u4tilde;
    %  control input for Truck 5
    u5tilde=Cui*xold(n0+4*ni+1:n0+5*ni)...
            -betatilde*kDi*g/(1-betatilde*kDi)*PP5(k1-1,6)...
            -kDi/(1-betatilde*kDi)*VP5(k1-1,5);
    if u5tilde>a5max
        u5tilde=a5max;
    end
    UP5(k1-1,6)=u5tilde;
    %  new overall state
    xnew=AP5*xold+BP5*UP5(k1-1,:)'+EP5*PP5(k1-1,:)'+BwP5*Vref(k1-1,1);
    newU=0;                % indicator for corrected input
    if xnew(1,1)>vmaxplus  % reduce the control input if the 
                           % velocity exceeds the limit
       deltaU=(vmaxplus-xnew(1,1))/BP5(1,1);  % correction term
       UP5(k1-1,1)=UP5(k1-1,1)+deltaU;      % corrected control input
       newU=1;
    end
    if xnew(n0+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+1,2);  % correction term
        UP5(k1-1,2)=UP5(k1-1,2)+deltaU;      % corrected control input
        newU=1;
    end
    if xnew(n0+ni+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+ni+1,3);  % correction term
        UP5(k1-1,3)=UP5(k1-1,3)+deltaU;      % corrected control input
        newU=1;
    end
    if xnew(n0+2*ni+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+2*ni+1,4);  % correction term
        UP5(k1-1,4)=UP5(k1-1,4)+deltaU;      % corrected control input
        newU=1;
    end
    if xnew(n0+3*ni+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+3*ni+1,5);  % correction term
        UP5(k1-1,5)=UP5(k1-1,5)+deltaU;      % corrected control input
        newU=1;
    end
    if xnew(n0+4*ni+1,1)>vmaxplus  % reduce the control input if the 
                              % velocity exceeds the limit
        deltaU=(vmaxplus-xnew(n0+1,1))/BP5(n0+4*ni+1,6);  % correction term
        UP5(k1-1,6)=UP5(k1-1,6)+deltaU;      % corrected control input
        newU=1;
    end
    if newU==1
       xnew=AP5*xold+BP5*UP5(k1-1,:)'+EP5*PP5(k1-1,:)'+BwP5*Vref(k1-1,1);
                                          % repeat to determine xnew
    end
    if u0tilde==amax
       xnew(3,1)=xold(3,1);   % anti-windup: integrator is not changed
    end
    XP5(k1,:)=xnew';        %  store as row vector
    DP5(k1,:)=CPd5*XP5(k1,:)';
    SP5(k1,:)=[CPs5*XP5(k1,:)' zeros(1,N1)];
    for k2=1:N1
        SP5(k1,k2+1)=SP5(k1,k2)-trucklength-alpha-DP5(k1,k2);
    end
    PP5(k1,:)=[roadprofile(SP5(k1,1)) zeros(1,N1)];
    for k2=1:N1
        PP5(k1,k2+1)=roadprofile(SP5(k1,k2+1));
    end
    VP5(k1,:)=CPv5*XP5(k1,:)';
    Vref(k1,1)=vmax;     % vref
end
%  simplified version to get the last (unimportant) value of U5
UP5(k1,:)=UP5(k1-1,:);
%
subplot(4,1,[1 2])
plot(Time0, SP5(:,1:N1), 'b');
latexylabel('$$s_i$$ in m $$\;$$');
rotateY;
xleer;
axis([180 230 2700 5000]);
yticks([0 3000 4000 5000]);
xticksempty([0 50 100 180 200 220 240]);

%%
%
epsfigc15('Trucksselforg');


