%      PlatoonApplSt.M
%
%   Application study with externally positive vehicles
%
%
% J. Lunze
% 25.7.2021
%
% for 2nd edition: 1.8.2021
% 
%
echo off
clear
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Vehicle parameters
%
T=1;   % Time constant of power train  in s
beta=2;
alpha=1;
Tend=5;

%  Vehicle model
A=[0 1 0 0; 0 -1/T 0 0; -1 0 0 0; beta 0 -1 0];
B=[0; 1/T; 0; 0];
C=[1 0 0 0];        % output: velocity
Cd=[0 0 1 0];       % output: distance
D=0;
E=[0; 0; 1; 0];     % input v_{i-1}


%%  Design of the feedback controller
%
figure(1)
Lambda=[-1 -1.5 -2 -2.5];
K=place(A, B, Lambda);
Abar=A-B*K;
VehicleG=ss(Abar, E, C, D);
VehicleF=ss(Abar, E, Cd, D);
[P,Z]=pzmap(VehicleG)
[Y,Time]=impulse(VehicleG,Tend);
%
subplot(4,1,[1 2]);
plot(Time, Y, 'b', [0 Tend], [0 0], 'k:');
latexylabel('$$\bar{g}$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonApplSt:}$$ Impulse response of the controlled vehicle');
axis([0 Tend -0.05 0.6]);
yticks([0 0.2 0.4 0.6 1]);
%
poly(Abar)    % coefficients of the characteristic polynomial



%%   Acceleration/braking manoeuvre
%     set-point changes for the leading vehicle
figure(2)
N1=9;
dt=0.2;
dt=0.2;
%  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]';
%
Integrator=tf(1, [1 0]);
%
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-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]);
%latextitle('$${\tt StringDistControl:}$$ Platoon with ACC');
axis([0 TendS -0.1 30]);
yticks([0 10 20 30]);
latextitle('$${\tt PlatoonApplSt:}$$ Vehicle platoon');
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 0 60]);
yticks([0 20 40 60]);
xticks([0 20 40 60 80]);
hold off
%



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Repetition of the design for the experimental robots
%
T=0.1;   % Time constant of power train  in s
beta=1;  % The model validation of the vehicle has been carried out for beta=1
alpha=0.1;
Tend=5;

%  Vehicle model
A=[0 1 0 0; 0 -1/T 0 0; -1 0 0 0; beta 0 -1 0];
B=[0; 1/T; 0; 0];
C=[1 0 0 0];        % output: velocity
Cd=[0 0 1 0];       % output: distance
D=0;
E=[0; 0; 1; 0];     % input v_{i-1}

%%  Design of the feedback controller
%
figure(3)
Lambda=[-1.5 -5 -7.5 -9];
K=place(A, B, Lambda);
Abar=A-B*K;
VehicleG=ss(Abar, E, C, D);
VehicleF=ss(Abar, E, Cd, D);
[P,Z]=pzmap(VehicleG)
[Y,Time]=impulse(VehicleG,Tend);
%
subplot(4,1,[1 2]);
plot(Time, Y, 'b--', [0 Tend], [0 0], 'k:');
hold on
latexylabel('$$\bar{g}$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonApplSt:}$$ Impulse response of the controlled vehicle');
axis([0 Tend -0.05 1]);
yticks([0 0.5 1]);
%     Comparison with experimental data
load('Messung_Gewichtsfunktion.mat')
% Relevanten Zeitbereich ausschneiden
t0 = 1;
te = 6;
id = t > t0 & t < te;
t = t(id)-t0;
vt = vt(:,id);
% Impulsantwort
plot(t,vt(2,:),'b');


%%   Acceleration/braking manoeuvre
%     set-point changes for the leading vehicle
%     the experiments have been carried out for beta=0.6
%     hence, the controller design has to be repeated
figure(4)
N1=8;     % 1 leader and 8 followers
dt=0.2;
beta=0.6;
A=[0 1 0 0; 0 -1/T 0 0; -1 0 0 0; beta 0 -1 0];
K=place(A, B, Lambda);
Abar=A-B*K;
VehicleG=ss(Abar, E, C, D);
VehicleF=ss(Abar, E, Cd, D);
[P,Z]=pzmap(VehicleG)
%  Velocity set-point w(t)
v0=0.1;        % initial velocity
v1=0.65;      % maximum velocity  0.75 m/s
Tv0=0;
Tv1=5;
Tv2=20;
Tv3=35;
TendS=50;
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+[0*ones(k1e-k0e,1);        % velocity of fictitious front car
              v1*ones(k2e-k1e,1);      % 0.75
             0*ones(k3e-k2e,1);      % 0
             v1*ones(k4e-k3e+1,1)];   % 0.75
TimeW=[0:dt:(k4e-1)*dt]';
%
Integrator=tf(1, [1 0]);
%
% initial state for v0=0.1
x0=[v0; 0; v0*beta; 0];   % for all vehicles
x0I=[v0; 0; 0; 0; 0];   % for all vehicles
S=[];
V=[];
D=[];
V0=lsim(VehicleG, W, TimeW,x0);
V=V0;
Vpred=V0;                                    % velocity of predecessor
S0=lsim(VehicleG*Integrator, W, TimeW, x0I);
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, x0);
    D(:,k1+1)=lsim(VehicleF,Vpred,TimeW, x0);
    S(:,k1+1)=Spred-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 10 20 30 40 50]);
%latextitle('$${\tt StringDistControl:}$$ Platoon with ACC');
axis([0 TendS -0.1 1]);
yticks([0 0.5 1]);
latextitle('$${\tt PlatoonApplSt:}$$ Vehicle platoon');
hold off
%
%  distances
subplot(4,1,[3 4]);
plot(TimeW, D(:,2:N1), 'b');
hold on
plot([0 TendS],[0 0],'k:');
latexylabel('$$\tilde{d}_i$$');    %   in m
rotateY;
latexxlabel('$$t$$ in s');
axis([0 TendS -0.03 0.6]);
yticks([0 0.2 0.4 0.6]);
xticks([0 10 20 30 40 50]);
hold off

%%   Experimental data (A. Schwab)
%
figure(5)
% Messdaten einlesen
load('Messung.mat')
% Relevanten Zeitbereich ausschneiden
t0 = 5;
te = 55;
id = t > t0 & t < te;
t = t(id)-t0;
vt = vt(:,id);
d = d(:,id);
% 
subplot(4,1,[1 2]);
% Geschwindigkeit
plot(t,vt,'b', t,v0(t+t0),'b--',  [0 TendS],[0 0],'k:');
latexylabel('$$v_i$$');   %    in $$\frac{\rm m}{\rm s}$$
rotateY;
xleer;
xticksempty([0 10 20 30 40 50]);
axis([0 TendS -0.1 1]);
yticks([0 0.5 1]);
latextitle('$${\tt PlatoonApplSt:}$$ Experimental data');
% reduzierter Abstand
subplot(4,1,[3 4]);
plot(t,d,'b')
hold on
plot([0 TendS],[0 0],'k:');
latexylabel('$$\tilde{d}_i$$');   %  in m
rotateY;
latexxlabel('$$t$$ in s');
axis([0 TendS -0.03 0.6]);
yticks([0 0.2 0.4 0.6]);
xticks([0 10 20 30 40 50]);
hold off



%%
%
epsfigc15('PlatoonApplSt');