%     Platoon.m
%
%   Abstandsregler für ein Fahrzeug mit Anwendung auf Fahrzeugkolonne
%
% aus  Script PlatoonIntMod.M  v. 9.8.2010 
%   
% J. Lunze  
% für 7. Auflage 24.5.2012
% für 8. Auflage 7.5.2014
% für 9. Auflage 4.5.2016
% für 10. Auflage: 9.9.2019
%
%
clear
close all
%%               Vehicle model with velocity and position control loops
m=1000;         % m= 1000 kg  mass
c=200;          %  c=100 kg/s friction constant
                % forces in N,  hence input to vehicle = acceleration in N
                % time in seconds
                % distance in m
                % velocity in m/s,   
                %         hence output  y=velocity in m/s
%
sbar=40;        % 40 m nominal distance between vehicles
%               PI velocity controller
kI=20;           % kI=5
kP=70;          % kP=10
%               Controlled vehicle
%               input u = set-point for velocity controller
%               output v = velocity
Aa=[-c/m-kP/m -kI/m; 1 0];
Ba=[kP/m; -1];
Ca=[1 0];
Da=0;
na=2;            % dynamical order of the controlled vehicle
%
%     Regelstreckenmodell
A=[Aa zeros(na,1); Ca 0];
B=[Ba; 0];
C=-[zeros(1,na), 1];  % dieser Ausgang: y-s_1
D=0;

Tend=200;


%%   Verhalten des Fahrzeugs mit P-Abstandsregelung
%
figure(1)
subplot(4,1,[1,2]);;
%
%    Fahrzeug mit P-Abstandsregler u=-kPs(y-w)              
kPs=-0.045;       % P-Abstandsregler
Aquer=A-B*kPs*C;
Bquer=B*kPs;      % in Zgl: + Bquer*w - Bquer*s_1
Cquer=C;          % in Agl: + s_1
Dquer=0;
nTS=3;                     % dynamische Ordnung des Fahrzeugs
Cars=ss(Aquer, Bquer, Cquer, Dquer);
[Y, T]=step(Cars,Tend);
subplot(4,1,[1,2]);
plot(T, Y, 'b', [0 Tend],[0 0],'k:');
latexxlabel('$$t$$  in s');
xticks([0 100 200]);
latexylabel('$$y$$ in m');  % y=-s_2 bei s_1=0, w=1
yticks([0 1]);
rotateY;
axis([0 Tend 0 1.2]);
latextitle('F\"uhrungs\"ubergangsfunktion des Abstandsregelkreises');


figure(2)
%
%    Störgrößenmodell
%
%
Ad=[0 1; 0 0];
Bd=[0; 0];
Cd=[1 0];
Dd=0;
Stoermodell=ss(Ad, Bd, Cd, Dd);
nd=2;
%  Anfangszustand bestimmt das Verhalten des F\"uhrungsfahrzeugs
vbar=10;         % Geschwindigkeit des 1. Fahrzeugs in m/s
s0=100;          % Startpunkt des 1. Fahrzeugs in m
xd0=[s0; vbar];
%
%   Modell für die beiden Fahrzeuge
%
Asim=[Ad zeros(nd,nTS); -Bquer*Cd  Aquer];
Bsim=[zeros(nd,1); Bquer];
Csim=[Cd Cquer;            % Fahrzeugabstand
      Cd zeros(1,nTS);      % Fahrzeugposition s1
      zeros(1,nd) -Cquer];  % Fahrzeugposition s2
Dsim=zeros(3,1);
Simmodell=ss(Asim, Bsim, Csim, Dsim);
%  Anfangszustand des 2. Fahrzeugs
s20=0;           % Startposition des 2. Fahrzeugs
v20=10;           % Geschwindigkeit des 2. Fahrzeugs
x0=[v20; -c/kI*v20; s20];% mit Nullzustand im Geschwindigkeitsregler
xsim0=[xd0; x0];
[Yinit, T]=initial(Simmodell, xsim0,Tend);
Ystep=step(Simmodell,T)*sbar;
Y=Yinit+Ystep;
%
subplot(4,1,[1,2]);
plot(T, Y(:,1), 'b', [0 Tend],[sbar, sbar],'k:');
xleer;
%latexxlabel('$$t$$  in s');
%xticks([0 100 200]);
latexylabel('$$y$$  in m');
yticks([0 100 200 300]);
rotateY;
axis([0 200 0 300]);
latextitle('Fahrzeugabstand bei P-Regelung');
%
%  Darstellung der beiden Fahrzeugpositionen
subplot(4,1,[3,4]);;
plot(T, Y(:,2), 'b--', T, Y(:,3), 'b');
latexxlabel('$$t$$  in s');
xticks([0 50 100 150 200]);
latexylabel('$$s_1, s_2$$');
%yticks([0 1]);
rotateY;
axis([0 200 -200 2000]);
latextext(50, 1000, '$$s_1$$');
latextext(50, 200, '$$s_2$$');
%latextitle('Fahrzeugpositionen');


%%  PI-Abstandsregler
%  Reglerentwurf siehe PlatoonIntMod.m
%   Ergebnis: ks=-0.028;  
%             k=0.000237;  
%             br=-1;
%
figure(3)
subplot(4,1,[1,2]);
kPs=-0.0606*1;       % neu: beide Faktoren zur Verhinderung des Überschwingens
kIs=-0.00063*0.7;     % Vorschlag von Yang Liu  
Aquer=[A-B*kPs*C  -B*kIs; C 0];
Bquer=[B*kPs; -1];     % in Zgl: + Bquer*w - Bquer*s_1
Cquer=[C 0];           % in Agl: + s_1
Dquer=0;
%   Regelkreismodell
CarIM=ss(Aquer, Bquer, Cquer, Dquer);
nTS=4;
[Ybar, Tbar]=step(CarIM,Tend);
plot(Tbar, Ybar, 'b');
latexxlabel('$$t$$  in s');
xticks([0 100 200]);
latexylabel('$$y$$  in m');
yticks([0 1]);
rotateY;
axis([0 Tend 0 1.3]);
latextitle('F\"uhrungs\"ubergangsfunktion mit PI-Abstandsregler');

%%   Modell für die beiden Fahrzeuge
%
figure(4)
Asim=[Ad zeros(nd,nTS); -Bquer*Cd  Aquer];
Bsim=[zeros(nd,1); Bquer];
Csim=[Cd Cquer;            % Fahrzeugabstand
      Cd zeros(1,nTS);     % Fahrzeugposition s1
      zeros(1,nd) -Cquer]; % Fahrzeugposition s2
Dsim=zeros(3,1);
Simmodell=ss(Asim, Bsim, Csim, Dsim);
%  Anfangszustand des 2. Fahrzeugs
x0=[v20; -c/kI*v20; s20; 0];% Anfangszustände der Regler aus stationärem 
                    % Verhalten
xsim0=[100; 10; x0];    % statt xd0
[Yinit, T]=initial(Simmodell, xsim0, Tend);
Ystep=step(Simmodell,T)*sbar;
Y=Yinit+Ystep;
subplot(4,1,[1,2]);;
plot(T, Y(:,1), 'b', [0 Tend],[sbar, sbar],'k:');
xleer;
%latexxlabel('$$t$$  in s');
%xticks([0 100 200]);
latexylabel('$$y$$  in m');
yticks([0 100 200 300]);
rotateY;
axis([0 200 0 300]);
latextitle('Fahrzeugsabstand bei PI-Regelung');
%
%  Darstellung der beiden Fahrzeugpositionen
subplot(4,1,[3,4]);;
plot(T, Y(:,2), 'b--', T, Y(:,3), 'b');
latexxlabel('$$t$$  in s');
xticks([0 50 100 150 200]);
latexylabel('$$s_1, s_2$$');
%yticks([0 1]);
rotateY;
axis([0 Tend -200 2000]);
latextext(100, 1500, '$$s_1$$');
latextext(100, 400, '$$s_2$$');
%latextitle('Fahrzeugpositionen');


%%  Fahrzeugkolonne mit PI-Abstandsregler
figure(5)
N=5;     % hinter F\"uhrungsfahrzeug fahren N weitere Fz.
Tend=250;
%
%  Fahrzeugmodelle: Aquer, Bquer, Cquer, 0
%  F\"uhrungsfahrzeug: Ad, Cd
Aplatoon=[Ad, zeros(nd, N*nTS); 
          zeros(N*nTS,nd) kron(diag(ones(N,1)), Aquer)];
Aplatoon(nd+1:nd+nTS,1:nd)=-Bquer*Cd;    % Kopplung vom 1. zum 2. Fz.
for k1=1:N-1    % Kopplung von Fz. k1 zu Fz. k1+1
    Aplatoon(nd+1+k1*nTS:nd+(k1+1)*nTS,nd+1+(k1-1)*nTS:nd+k1*nTS)=Bquer*Cquer; 
end
Bplatoon=[zeros(nd,1); kron(ones(N,1), Bquer)];
Cplatoon=[1 0 zeros(1, N*nTS);                 % Position d. 1. Fz.
          zeros(N, nd) kron(diag(ones(N,1)), -Cquer)];% Positionen aller anderen Fz.
Dplatoon=zeros(N+1, 1);
PlatoonModell=ss(Aplatoon, Bplatoon, Cplatoon, Dplatoon);
%  Initial states of the vehicles
%s0v=sort(600*rand([N, 1]),'descend');     % starting positions of the vehicles
v0v=12*ones(N,1); %-5*rand([N, 1]);     % starting velocity of the vehicles
x0=zeros(N*nTS, 1);
for k=1:N
    x0((k-1)*nTS+3,1)=400-k*130;        % s0v(k,1);
    x0((k-1)*nTS+1,1)=v0v(k,1);
    x0((k-1)*nTS+2,1)=-c/kI*v0v(k,1);
end
%  Initial states of the reference model
vbar=10;         % velocity 
s0=600;        % starting point
xd0=[s0; vbar];
x0platoon=[xd0; x0];
[Yplatoon, Tplatoon]=initial(PlatoonModell, x0platoon);
[Yinit, T]=initial(PlatoonModell, x0platoon, Tend);
Ystep=step(PlatoonModell,T)*sbar;
Y=Yinit+Ystep; 
plot(T, Y(:,1), 'b--');    % 1. Fahrzeug
hold on;
plot(T, Y(:,2:N+1), 'b');                      % alle anderen Fz.
hold off
latexxlabel('$$t$$  in s');
xticks([0 100 200]);
latexylabel('$$s_i$$ in m');
yticks([0 1000 2000 3000]);
rotateY;
axis([0 Tend -300 3000]);
latextitle('Fahrzeugkolonne');


%%  Überschwingen der Abstandsregelung beseitigen
%  Der PI-Regler wird um ein dynamisches Element erweitert, 
%  damit die Abstandsregelung ohne Überschwingen den Sollwert erreicht
%  (Forderung aus Kolonnenstabilität; dieser Teil ändert nichts
%  an der Sollwertfolge und ist deshalb nicht im Buch erläutert)
figure(6)
% Korrekturglied zum Anheben des Amplitudenganges und der Phase
%  in der Nähe der Schnittfrequenz
TD=0.1;
T1=0.3;
%kPs=-0.03;
%kIs=-0.0003;
Aerw=[A-B*kPs*TD/T1*C  -B*kIs -B*kPs;
    TD/T1*C      0     1;
    1/T1*C       0     -1/T1];
Berw=[B*kPs*TD/T1; -TD/T1; -1/T1];     % in Zgl: + Bquer*w - Bquer*s_1
Cerw=[C 0 0];           % in Agl: + s_1
Derw=0;
nTS=5;
%   Regelkreismodell
Carerw=ss(Aerw, Berw, Cerw, Derw);
[Ybar, Tbar]=step(Carerw,Tend);
plot(Tbar, Ybar, 'b');
latexxlabel('$$t$$  in s');
xticks([0 100 200]);
latexylabel('$$y$$  in m');
yticks([0 1]);
rotateY;
axis([0 Tend 0 1.3]);
latextitle('F\"uhrungs\"ubergangsfunktion mit PI-Abstandsregler');





%%
%   EPS-Bilder
%
Epsbildc15('Platoon');
