%   Script CarBrakeManoeuvre.m
%
%   Decentralised and cooperative control of a vehicle braking system
%
% J. Lunze
% 24.10.2019
% Version of 29.11.2019
% for NCSAppl: 19.5.2021
%
echo off
clear
close all

% Data from CARBRAKE.m (AT, 4. Aufl.)
%
g=9.81;         % m/s^2
m=250;          % kg  (Masse von 1/4 eines Fahrzeugs)
r=0.27;         % m  (Radius eines Rades)
ri=0.17;        % m  innerer Radius des Reifens
ra=r;           % m  äußerer Radius des Reifens
mr=25;          % kg  Reifenmasse
J=m*(ri*ri+ra*ra)/2;      % Massenträgheitsmoment eines Rades 
%                           (berechnet für einen Hohlzylinder, [Dubbel, S. B32])
%
v0=50/3.6;      % 50/3.6  m/s   Anfangsgeschwindigkeit 50 km/h
x0=0;           % m     Anfangsposition
omega0=v0/r;    % rad/s Anfangswinkelgeschwindigkeit des Rades
Fb=m*v0/2;      % kg m/s^2 (N)   Bremskraft für Halt nach 2 Sekunden
                %  verwendet für alle vier Räder
%
%
%               inertia with respect to yaw movement
carlength=5;       % m length of vehicle
carwidth= 2;       % m width of vehicle
mV=4*m;         % kg mass
JV=mV/12*(carlength^2+carwidth^2);  %  formula for cuboid



%%   Schlupfkennlinie von CARBRAKE.m, für AT, 5. Auflage v. 12.11.2019
%
figure(1)
subplot(1,4,[1,2,3]);
%  Formel und Parameter aus Kiencke/Nielson S. 241
c1Asphalt=1.2801;   % trockener Asphalt
c2Asphalt=23.99;
c3Asphalt=0.52;
c1Nass=0.857;       % nasser Asphalt
c2Nass=33.822;
c3Nass=0.347;
c1Eis=0.1946;       % Eis
c2Eis=94.129;
c3Eis=0.0646;

lambda=[0:0.005:1];
muAsphalt=c1Asphalt*(ones(size(lambda))-exp(-c2Asphalt*lambda))-c3Asphalt*lambda;
muHAsphalt=max(muAsphalt);
muNass=c1Nass*(ones(size(lambda))-exp(-c2Nass*lambda))-c3Nass*lambda;
muHNass=max(muNass);
muEis=c1Eis*(ones(size(lambda))-exp(-c2Eis*lambda))-c3Eis*lambda;
muHEis=max(muEis);
plot(lambda, muAsphalt, 'b', lambda, muNass, 'b',...
     lambda, muEis, 'b', [0 0.25], [max(muAsphalt), max(muAsphalt)], 'k--');
axis([0 1 0 1.2]);
yticks([0 0.2 0.4 0.6 0.8 1 ]);
xticks([0 0.2  0.4 0.6 0.8 1]);
latextitle('Examples for the friction-slip curve');
latextext(0.6, 1.05, 'dry asphalt');
latextext(0.6, 0.7, 'wet asphalt');
latextext(0.6, 0.2, 'icy road');
%latextext(1.02, muAsphalt(length(lambda)), '$$\mu_{\rm G}$$');
latextext(-0.1, max(muAsphalt), '$$\mu_{\rm H}$$');
latexxlabel('$$\lambda$$');
latexylabel('$$\mu$$');
rotateY;
latextitle('$${\tt CarBrakeManoeuvre:}$$ Friction-slip curves');
%
%  Approximation by linear curves
subplot(1,4,4)
plot(lambda, muAsphalt, 'b--', lambda, muNass, 'b--',...
     lambda, muEis, 'b--');
hold on
plot([0 0.2], 115*1.2/8*[0 0.2],'b', [0 0.2], 115*1.2/13*[0 0.2],'b');
axis([0 0.2 0 1.2]);
xticks([0 0.1 0.2]);
yleer;
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Braking manoeuvre with constant braking force
%
figure(2)
%  Assumptions
%  kmu - linearised mu/lambda-curve  kmu1,..., kmu4
%  Fx1=Fx2;  Fx3=Fx4,   hence  Foverall=2*Fx1-2*Fx3;
kmu1=1.2/8*115;    %  right wheels on dry asphalt
kmu2=kmu1;
kmu3=1.2/13*115;   %  left wheels on ice
kmu4=kmu3;
%  Initial values
Tend=5;           % model becomes invalid if velocity is too small, hence Tend<3.4
dt=0.001;
Time=[0:dt:Tend];
u1bar=2*Fb/m;        % commanded acceleration for each wheel
u1=u1bar*ones(size(Time));
u3=u1;
%  Right wheels  1, 2
omega1=[];
omega1(1)=omega0;
lambda1=[];
Fx1=[];
%  Left wheels    3, 4
omega3=[];
omega3(1)=omega0;
lambda3=[];
Fx3=[];
% Velocity
v=[];
v(1)=v0;
%   Euler integration of the differential equations
for k1=1:length(Time)-1
    if v(k1)<0.1
        v(k1+1)=0;
        omega1(k1+1)=0;
        omega3(k1+1)=0;
        lambda1(k1)=0;
        lambda3(k1)=0;
        Fx1(k1)=0;
        Fx3(k1)=0;
    else
         if omega1(k1)<0.1    % blocking wheel
            omega1(k1)=0;
            lambda1(k1)=1;
        else
        lambda1(k1)=1-r/v(k1)*omega1(k1);
        end
        if omega3(k1)<0.1    % blocking wheel
            omega3(k1)=0;
            lambda3(k1)=1;
        else
        lambda3(k1)=1-r/v(k1)*omega3(k1);
        end
        omega1(k1+1)=omega1(k1) + (m*r/J*g*kmu1*lambda1(k1) - m*r/J*u1(k1))*dt;
        Fx1(k1)=g*kmu1*lambda1(k1)*m;
        omega3(k1+1)=omega3(k1) + (m*r/J*g*kmu3*lambda3(k1) - m*r/J*u3(k1))*dt;
        Fx3(k1)=g*kmu3*lambda3(k1)*m;
        v(k1+1)=v(k1)-(2*Fx1(k1)+2*Fx3(k1))/4/m*dt;
    end
end
Fx1(k1+1)=g*kmu1*lambda1(k1)*m;   % to get a vector of length(Time)
Fx3(k1+1)=g*kmu3*lambda3(k1)*m;
%   Yaw movement of the vehicle
phi=[];
phi(1)=pi/2;
for k1=1:length(Time)-1
    phi(k1+1)=phi(k1)+carwidth/2/JV*2*(-Fx1(k1)+Fx3(k1))*dt;
end
%   Movement of the vehicle
sx=[];    % longitudinal direction
sy=[];    % lateral movement
sx(1)=0;
sy(1)=0;
for k1=1:length(v)
    sy(k1+1)=sy(k1)+v(k1)*dt*cos(phi(k1));
    sx(k1+1)=sx(k1)+v(k1)*dt*sin(phi(k1));
end
%
figure(2)
subplot(2,1,1)
plot(Time, omega1,'b', Time, omega3,'b--');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Linear model, constant braking force');
latexylabel('$$\omega_1, \omega_3$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
xleer;
yticks([0 20 40 60]);
axis([0 Tend 0 60]);
%
subplot(2,1,2)
plot(Time, v,'b');
latexylabel('$$v$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend 0 15]);
xticks([0 1 2 3 4 5 6]);
yticks([0 5 10 15]);
%
figure(3)
plot(sy, sx, 'b--');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Linear model');
hold on


%%   Braking manoeuvre with controlled braking force
%
figure(4);    
kgain=10;     % controller gain, u(k)= -kdec(ubar-ui) +/- kgain*phi(k)
kdec=2;    % gain of decentralised controller
Tend=5;           % model becomes invalid if velocity is too small, hence Tend<3.4
dt=0.001;
Time=[0:dt:Tend];
% Wheels right
lambda1=[];
omega1=[];
omega1(1)=omega0;
Fx1=[];
% Wheels left
lambda3=[];
omega3=[];
omega3(1)=omega0;
Fx3=[];
% Velocity
v=[];
v(1)=v0;
phi=[];
phi(1)=pi/2;
Fx1(1)=0;
Fx3(1)=0;
% control inputs
u1=[];
u1(1)=kdec*(u1bar-Fx1(1)/m);
u3=[];
u3(1)=kdec*(u1bar-Fx3(1)/m);
%   Euler integration of the differential equations
for k1=1:length(Time)-1
    if v(k1)<0.1
        v(k1+1)=0;
        lambda1(k1)=0;
        lambda3(k1)=0;
        omega1(k1+1)=0;
        omega3(k1+1)=0;
        Fx1(k1)=0;
        Fx3(k1)=0;
        phi(k1+1)=phi(k1);
        u1(k1+1)=u1(k1);
        u3(k1+1)=u3(k1);
    else
        if omega1(k1)<0.1    % blocking wheel
            omega1(k1)=0;
            lambda1(k1)=1;
        else
        lambda1(k1)=1-r/v(k1)*omega1(k1);
        end
         if omega3(k1)<0.1    % blocking wheel
            omega3(k1)=0;
            lambda3(k1)=1;
        else
        lambda3(k1)=1-r/v(k1)*omega3(k1);
        end
        omega1(k1+1)=omega1(k1) + (m*r/J*g*kmu1*lambda1(k1) - m*r/J*u1(k1))*dt;
        Fx1(k1)=g*kmu1*lambda1(k1)*m;
        omega3(k1+1)=omega3(k1) + (m*r/J*g*kmu3*lambda3(k1) - m*r/J*u3(k1))*dt;
        Fx3(k1)=g*kmu3*lambda3(k1)*m;
        v(k1+1)=v(k1)-(2*Fx1(k1)+2*Fx3(k1))/mV*dt;
        phi(k1+1)=phi(k1)+carwidth/2/JV*2*(-Fx1(k1)+Fx3(k1))*dt;
        u1(k1+1)=kdec*(u1bar-Fx1(k1)/m)+kgain*(phi(k1)-pi/2);
        u3(k1+1)=kdec*(u1bar-Fx3(k1)/m)-kgain*(phi(k1)-pi/2);
    end
end
lambda1(k1+1)=lambda1(k1);
lambda3(k1+1)=lambda3(k1);
Fx1(k1+1)=g*kmu1*lambda1(k1)*m;
Fx3(k1+1)=g*kmu3*lambda3(k1)*m;
%   Movement of the vehicle
sx=[];
sy=[];
sx(1)=0;
sy(1)=0;
for k1=1:length(v)-1
    sy(k1+1)=sy(k1)+v(k1)*dt*cos(phi(k1));
    sx(k1+1)=sx(k1)+v(k1)*dt*sin(phi(k1));
end
%
figure(4)
subplot(2,1,1)
plot(Time, omega1,'b', Time, omega3,'b--');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Linear model, controlled braking force');
latexylabel('$$\omega_1, \omega_3$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
xleer;
yticks([0 20 40 60]);
axis([0 Tend 0 60]);
%
subplot(2,1,2)
plot(Time, v,'b');
latexylabel('$$v$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend 0 15]);
xticks([0 1 2 3 4 5 6]);
yticks([0 5 10 15]);
%
figure(3)
plot(sy, sx, 'b');
hold off
latexylabel('$$s$$ in m');
latexxlabel('$$y_{\rm K}$$ in m');
rotateY;
xticks([0 0.1 0.2 0.3 0.4 0.5]);
yticks([0 10 20 30 40]);
%
figure(5)
subplot(2,1,1)
plot(Time, lambda1, 'b', Time, lambda3, 'b--');
axis([0 Tend 0 1.1*max(lambda3)]);
latexxlabel('$$t$$ in s');
latexylabel('$$\lambda_1, \lambda_3$$');
rotateY;
xticks([0 1 2 3 4]);
yticks([0 0.05 0.1 0.15 0.2 0.25 0.3]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Nonlinear model
%
figure(6)
%  Assumptions
%  Fx1=Fx2;  Fx3=Fx4,   hence  Foverall=2*Fx1-2*Fx3;
%
%  Constant braking acceleration
dt=0.005;
u1bar=Fb/m;
u1=u1bar;
u3=u1;
%   Euler integration of the differential equations
% Right wheels
omega1=[];
omega1(1)=omega0;
Fx1=[];
mu1=[];
lambda1=[];
% Left wheels 
omega3=[];
omega3(1)=omega0;
Fx3=[];
mu3=[];
lambda3=[];
v=[];
v(1)=v0;
k1=1;
%  Euler integration, variable time horizon, 
%  integration stops for v<vmin m/s
vmin=0.3;
while v(k1)>vmin && k1<2000 
    if omega1(k1)<1.1*vmin/r    % blocking wheel
       omega1(k1)=0;
       lambda1(k1)=1;
    else
       lambda1(k1)=1-r/v(k1)*omega1(k1);
    end
    if omega3(k1)<1.1*vmin/r    % blocking wheel
       omega3(k1)=0;
       lambda3(k1)=1;
    else
       lambda3(k1)=1-r/v(k1)*omega3(k1);
    end
    mu1(k1)=c1Asphalt-exp(-c2Asphalt*lambda1(k1))-c3Asphalt*lambda1(k1);
    mu3(k1)=c1Nass-exp(-c2Nass*lambda3(k1))-c3Nass*lambda3(k1);
    omega1(k1+1)=omega1(k1) + (m*r/J*g*mu1(k1) - m*r/J*u1)*dt;
    Fx1(k1)=g*mu1(k1)*m;
    omega3(k1+1)=omega3(k1) + (m*r/J*g*mu3(k1) - m*r/J*u3)*dt;
    Fx3(k1)=g*mu3(k1)*m;
    v(k1+1)=v(k1)-(2*Fx1(k1)+2*Fx3(k1))/mV*dt;
    k1=k1+1;
end
lambda1(k1)=lambda1(k1-1);
mu1(k1)=mu1(k1-1);
Fx1(k1)=Fx1(k1-1);
lambda3(k1)=lambda3(k1-1);
mu3(k1)=mu3(k1-1);
Fx3(k1)=Fx3(k1-1);
Tend=k1*dt;
Time=[0:dt:Tend-dt];
%   Yaw movement of the vehicle
phi=[];
phi(1)=pi/2;
for k1=1:length(v)-1
    phi(k1+1)=phi(k1)+carwidth/2/JV*2*(-Fx1(k1)+Fx3(k1))*dt;
end
%   Movement of the vehicle
sx=[];
sy=[];
sx(1)=0;
sy(1)=0;
for k1=1:length(v)
    sy(k1+1)=sy(k1)+v(k1)*dt*cos(phi(k1));
    sx(k1+1)=sx(k1)+v(k1)*dt*sin(phi(k1));
end
%
figure(6)
subplot(4,1,[1 2])
plot(Time, omega1,'b', Time, omega3,'b--');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Nonlinear model, constant braking force');
latexylabel('$$\omega_1, \omega_3 \;$$'); %  in $$\frac{\rm rad}{\rm s}$$
rotateY;
xleer;
yticks([0 20 40 60]);
axis([0 Tend 0 60]);
%
subplot(4,1,[3 4])
plot(Time, v,'b');
latexylabel('$$v \;$$'); %  in $$\frac{\rm m}{\rm s}$$
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend 0 15]);
xticks([0 1 2 3 4 5 6]);
yticks([0 5 10 15]);
%
figure(7)
plot(sy, sx, 'b--');
hold on
plot([0 0],[0 30],'k:');

%%   Braking manoeuvre with controlled braking force
%
figure(8);  
kgain=10;     % controller gain, u(k)= -kdec(ubar-ui) +/- kgain*phi(k)
kdec=2;    % gain of decentralised controller
% Right wheels
omega1=[];
omega1(1)=omega0;
Fx1=[];
mu1=[];
lambda1=[];
% Left wheels 
omega3=[];
omega3(1)=omega0;
Fx3=[];
mu3=[];
lambda3=[];
%
v=[];
v(1)=v0;
phi=[];
phi(1)=pi/2;
u1=[];
u1(1)=u1bar;
u3=[];
u3(1)=u1(1);
k1=1;
%  Euler integration, variable time horizon, 
%  integration stops for v<vmin m/s
vmin=0.3;
while v(k1)>vmin && k1<2000
    if omega1(k1)<1.1*vmin/r    % blocking wheel
       omega1(k1)=0;
       lambda1(k1)=1;
    else
       lambda1(k1)=1-r/v(k1)*omega1(k1);
    end
    if omega3(k1)<1.1*vmin/r    % blocking wheel
       omega3(k1)=0;
       lambda3(k1)=1;
    else
       lambda3(k1)=1-r/v(k1)*omega3(k1);
    end
    mu1(k1)=c1Asphalt-exp(-c2Asphalt*lambda1(k1))-c3Asphalt*lambda1(k1);
    omega1(k1+1)=omega1(k1) + (m*r/J*g*mu1(k1) - m*r/J*u1(k1))*dt;
    Fx1(k1)=g*mu1(k1)*m;
    mu3(k1)=c1Nass-exp(-c2Nass*lambda3(k1))-c3Nass*lambda3(k1);
    omega3(k1+1)=omega3(k1) + (m*r/J*g*mu3(k1) - m*r/J*u3(k1))*dt;
    Fx3(k1)=g*mu3(k1)*m;
    v(k1+1)=v(k1)-(2*Fx1(k1)+2*Fx3(k1))/mV*dt;
    phi(k1+1)=phi(k1)+carwidth/2/JV*2*(-Fx1(k1)+Fx3(k1))*dt;
    % controller
    u1(k1+1)=kdec*(u1bar-Fx1(k1)/m)+kgain*(phi(k1)-pi/2);
    u3(k1+1)=kdec*(u1bar-Fx3(k1)/m)-kgain*(phi(k1)-pi/2);
    k1=k1+1;
end
lambda1(k1)=lambda1(k1-1);
mu1(k1)=mu1(k1-1);
Fx1(k1)=Fx1(k1-1);
lambda3(k1)=lambda3(k1-1);
mu3(k1)=mu3(k1-1);
Fx3(k1)=Fx3(k1-1);
Tend=k1*dt;
Time=[0:dt:Tend-dt];
%   Movement of the vehicle
sx=[];
sy=[];
sx(1)=0;
sy(1)=0;
for k1=1:length(v)
    sy(k1+1)=sy(k1)+v(k1)*dt*cos(phi(k1));
    sx(k1+1)=sx(k1)+v(k1)*dt*sin(phi(k1));
end
%
figure(8)
subplot(4,1,[1,2])
plot(Time, omega1,'b', Time, omega3,'b--');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Nonlinear model, controlled braking force');
latexylabel('$$\omega_1, \omega_3 \;$$'); % in $$\frac{\rm rad}{\rm s}$$
rotateY;
xleer;
xticksempty([0 1 2 3 4 5 6]);
yticks([0 20 40 60]);
axis([0 Tend 0 60]);
%
subplot(4,1,[3 4])
plot(Time, v,'b');
latexylabel('$$v \;$$'); %  in $$\frac{\rm m}{\rm s}$$
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend 0 15]);
xticks([0 1 2 3 4 5 6]);
yticks([0 5 10 15]);
%
figure(7)
plot(sy, sx, 'b');
hold on
latexylabel('$$s$$ in m');
latexxlabel('$$y_{\rm K}$$ in m');
rotateY;
xticks([0 0.5 1 1.5 2 2.5]);
yticks([0 10 20 30 40]);
axis([-0.05 0.7 0 30]);
hold off
%
figure(9)
subplot(2,1,1)
plot(Time, lambda1, 'b', Time, lambda3, 'b--');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Nonlinear model, controlled braking force');
axis([0 Tend 0 0.07]);
latexxlabel('$$t$$ in s');
latexylabel('$$\lambda_1, \lambda_3$$');
rotateY;
xticks([0 1 2 3 ]);
yticks([0 0.05 0.1]);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Comparison of nonlinear and linearised model 
%   Braking manoeuvre with constant braking force
%
figure(10)
%  Assumptions
%  kmu - linearised mu/lambda-curve  kmu1,..., kmu4
%  Fx1=Fx2=Fx3=Fx4,   hence  Foverall=4*Fx1;
%  Initial values
Tend=2;           % model becomes invalid if velocity is too small, hence Tend<3.4
dt=0.001;
Time=[0:dt:Tend];
u1bar=2*Fb/m;        % commanded acceleration for each wheel
                   %  with Fb=m*v0/2;
u1=u1bar*ones(size(Time));
%  All wheels 
lambda1=[];
mu1=[];
omega1=[];
omega1(1)=omega0;
Fx1=[];
% Velocity
v=[];
v(1)=v0;
%   Euler integration of the differential equations
vmin=0.2;
for k1=1:length(Time)-1
    if v(k1)<vmin             % vehicle stands still
        v(k1+1)=0;
        lambda1(k1)=0;
        mu1(k1)=0;
        omega1(k1+1)=0;
        Fx1(k1)=0;
    else
        if omega1(k1)<1.1*vmin/r
            omega1(k1)=0;
            lambda1(k1)=1;
        else
        lambda1(k1)=1-r/v(k1)*omega1(k1);
        end
        mu1(k1)=c1Asphalt-exp(-c2Asphalt*lambda1(k1))-c3Asphalt*lambda1(k1);
       % mu1(k1)=c1Nass-exp(-c2Nass*lambda1(k1))-c3Nass*lambda1(k1);
       % mu1(k1)=c1Eis-exp(-c2Eis*lambda1(k1))-c3Eis*lambda1(k1);
        omega1(k1+1)=omega1(k1) + (m*r/J*g*mu1(k1) - m*r/J*u1(k1))*dt;
        Fx1(k1)=g*mu1(k1)*m;
        v(k1+1)=v(k1)-4*Fx1(k1)/mV*dt;
    end
end
lambda1(k1+1)=lambda1(k1);
%
figure(10)
subplot(2,1,1)
plot(Time, omega1,'b');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Nonlinear model, constant braking force');
latexylabel('$$\omega_1$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
xleer;
yticks([0 20 40 60]);
axis([0 Tend 0 60]);
hold on
%
subplot(2,1,2)
plot(Time, v,'b');
latexylabel('$$v$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend 0 15]);
xticks([0 1 2 3 4 5 6]);
yticks([0 5 10 15]);
hold on
%
figure(11)
subplot(2,1,1)
plot(Time, lambda1,'b');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Nonlinear model, constant braking force');
latexylabel('$$\lambda$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 1 2 3]);
yticks([0 0.05 0.1]);
axis([0 Tend 0 0.1]);
hold on

% Comparison with the linearised model
figure(10)
kmu1=1.2/8*115;    %  right wheels on dry asphalt
kmu3=1.2/13*115;   %  left wheels on ice
kmu4=kmu3;
%  All wheels 
lambda1=[];
lambda1(1)=0;
omega1=[];
omega1(1)=omega0;
Fx1=[];
% Velocity
v=[];
v(1)=v0;
%   Euler integration of the differential equations
vmin=0.2;
for k1=1:length(Time)-1
    if v(k1)<vmin
        v(k1+1)=0;
        lambda1(k1)=0;
        omega1(k1+1)=0;
        Fx1(k1)=0;
    else
        lambda1(k1)=1-r/v(k1)*omega1(k1);
        omega1(k1+1)=omega1(k1) + (m*r/J*g*kmu1*lambda1(k1) - m*r/J*u1(k1))*dt;
        Fx1(k1)=g*kmu1*(1-r/v(k1)*omega1(k1))*m;
        v(k1+1)=v(k1)-4*Fx1(k1)/mV*dt;
    end
end
lambda1(k1+1)=lambda1(k1);
subplot(2,1,1)
plot(Time, omega1,'r--');
hold off
%
subplot(2,1,2)
plot(Time, v,'r--');
hold off
%
figure(11)
plot(Time, lambda1,'r--');
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Changing road conditions:
%  0 ... 10 m:   left: icy road;  right: dry asphalt
%  10 m ...  :   left: dry asphalt;  right: icy road
schange=7;   % numerical problems with the controlled vehicle for longer s
%
figure(12)
%  Assumptions
%  Fx1=Fx2;  Fx3=Fx4,   hence  Foverall=2*Fx1-2*Fx3;
%
%  Constant braking acceleration
dt=0.005;
u1bar=Fb/m;
u1=u1bar;
u3=u1;
% Right wheels
omega1=[];
omega1(1)=omega0;
Fx1=[];
mu1=[];
lambda1=[];
% Left wheels 
omega3=[];
omega3(1)=omega0;
Fx3=[];
mu3=[];
lambda3=[];
v=[];
v(1)=v0;
%   Movement of the vehicle
sx=[];
sy=[];
sx(1)=0;
sy(1)=0;
%   Yaw movement of the vehicle
phi=[];
phi(1)=pi/2;
%  Euler integration, variable time horizon, 
%  integration stops for v<vmin m/s
k1=1;
vmin=0.3;
while v(k1)>vmin && sx(k1)<schange
    if omega1(k1)<1.1*vmin/r    % blocking wheel
       omega1(k1)=0;
       lambda1(k1)=1;
    else
       lambda1(k1)=1-r/v(k1)*omega1(k1);
    end
    if omega3(k1)<1.1*vmin/r    % blocking wheel
       omega3(k1)=0;
       lambda3(k1)=1;
    else
       lambda3(k1)=1-r/v(k1)*omega3(k1);
    end
    mu1(k1)=c1Asphalt-exp(-c2Asphalt*lambda1(k1))-c3Asphalt*lambda1(k1);
    mu3(k1)=c1Nass-exp(-c2Nass*lambda3(k1))-c3Nass*lambda3(k1);
    omega1(k1+1)=omega1(k1) + (m*r/J*g*mu1(k1) - m*r/J*u1)*dt;
    Fx1(k1)=g*mu1(k1)*m;
    omega3(k1+1)=omega3(k1) + (m*r/J*g*mu3(k1) - m*r/J*u3)*dt;
    Fx3(k1)=g*mu3(k1)*m;
    v(k1+1)=v(k1)-(2*Fx1(k1)+2*Fx3(k1))/mV*dt;
    %  vehicle movement
    phi(k1+1)=phi(k1)+carwidth/2/JV*2*(-Fx1(k1)+Fx3(k1))*dt;
    sy(k1+1)=sy(k1)+v(k1)*dt*cos(phi(k1));
    sx(k1+1)=sx(k1)+v(k1)*dt*sin(phi(k1));
    k1=k1+1;
end
%  change of the road conditions
kchange=k1;
while v(k1)>vmin && k1<2000
    if omega1(k1)<1.1*vmin/r    % blocking wheel
       omega1(k1)=0;
       lambda1(k1)=1;
    else
       lambda1(k1)=1-r/v(k1)*omega1(k1);
    end
    if omega3(k1)<1.1*vmin/r    % blocking wheel
       omega3(k1)=0;
       lambda3(k1)=1;
    else
       lambda3(k1)=1-r/v(k1)*omega3(k1);
    end
    % wheel 3 on dry asphalt
    mu3(k1)=c1Asphalt-exp(-c2Asphalt*lambda3(k1))-c3Asphalt*lambda3(k1);
    % wheel 1 on icy road
    mu1(k1)=c1Nass-exp(-c2Nass*lambda1(k1))-c3Nass*lambda1(k1);
    % the same as before
    omega1(k1+1)=omega1(k1) + (m*r/J*g*mu1(k1) - m*r/J*u1)*dt;
    Fx1(k1)=g*mu1(k1)*m;
    omega3(k1+1)=omega3(k1) + (m*r/J*g*mu3(k1) - m*r/J*u3)*dt;
    Fx3(k1)=g*mu3(k1)*m;
    v(k1+1)=v(k1)-(2*Fx1(k1)+2*Fx3(k1))/mV*dt;
    %  vehicle movement
    phi(k1+1)=phi(k1)+carwidth/2/JV*2*(-Fx1(k1)+Fx3(k1))*dt;
    sy(k1+1)=sy(k1)+v(k1)*dt*cos(phi(k1));
    sx(k1+1)=sx(k1)+v(k1)*dt*sin(phi(k1));
    k1=k1+1;
end
lambda1(k1)=lambda1(k1-1);
mu1(k1)=mu1(k1-1);
Fx1(k1)=Fx1(k1-1);
lambda3(k1)=lambda3(k1-1);
mu3(k1)=mu3(k1-1);
Fx3(k1)=Fx3(k1-1);
Tend=k1*dt;
Time=[0:dt:Tend-dt];
%
figure(12)
subplot(2,1,1)
plot(Time, omega1,'b', Time, omega3,'b--');
hold on
plot([kchange*dt kchange*dt],[0 60], 'k:');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Changing road conditions');
latexylabel('$$\omega_1, \omega_3 \;$$'); %  in $$\frac{\rm rad}{\rm s}$$
rotateY;
xleer;
yticks([0 20 40 60]);
axis([0 Tend 0 60]);
hold off
%
subplot(2,1,2)
plot(Time, v,'b');
hold on
plot([kchange*dt kchange*dt],[0 15], 'k:');
latexylabel('$$v \;$$'); %  in $$\frac{\rm m}{\rm s}$$
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend 0 15]);
xticks([0 1 2 3 4 5 6]);
yticks([0 5 10 15]);
hold off
%
figure(13)
plot(sy, sx, 'b--');
hold on
plot([0 0],[0 30],'k:');



%%   Braking manoeuvre with controlled braking force
%
figure(14);  
kgain=10;     % controller gain, u(k)= -kdec(ubar-ui) +/- kgain*phi(k)
kdec=2;    % gain of decentralised controller
% Right wheels
omega1=[];
omega1(1)=omega0;
Fx1=[];
mu1=[];
lambda1=[];
% Left wheels 
omega3=[];
omega3(1)=omega0;
Fx3=[];
mu3=[];
lambda3=[];
%
v=[];
v(1)=v0;
phi=[];
phi(1)=pi/2;
u1=[];
u1(1)=u1bar;
u3=[];
u3(1)=u1(1);
k1=1;
%   Movement of the vehicle
sx=[];
sy=[];
sx(1)=0;
sy(1)=0;
%  Euler integration, variable time horizon, 
%  integration stops for v<vmin m/s
vmin=0.3;
while v(k1)>vmin && sx(k1)<schange
    if omega1(k1)<1.1*vmin/r    % blocking wheel
       omega1(k1)=0;
       lambda1(k1)=1;
    else
       lambda1(k1)=1-r/v(k1)*omega1(k1);
    end
    if omega3(k1)<1.1*vmin/r    % blocking wheel
       omega3(k1)=0;
       lambda3(k1)=1;
    else
       lambda3(k1)=1-r/v(k1)*omega3(k1);
    end
    mu1(k1)=c1Asphalt-exp(-c2Asphalt*lambda1(k1))-c3Asphalt*lambda1(k1);
    omega1(k1+1)=omega1(k1) + (m*r/J*g*mu1(k1) - m*r/J*u1(k1))*dt;
    Fx1(k1)=g*mu1(k1)*m;
    mu3(k1)=c1Nass-exp(-c2Nass*lambda3(k1))-c3Nass*lambda3(k1);
    omega3(k1+1)=omega3(k1) + (m*r/J*g*mu3(k1) - m*r/J*u3(k1))*dt;
    Fx3(k1)=g*mu3(k1)*m;
    v(k1+1)=v(k1)-(2*Fx1(k1)+2*Fx3(k1))/mV*dt;
    phi(k1+1)=phi(k1)+carwidth/2/JV*2*(-Fx1(k1)+Fx3(k1))*dt;
    % controller
    u1(k1+1)=kdec*(u1bar-Fx1(k1)/m)+kgain*(phi(k1)-pi/2);
    u3(k1+1)=kdec*(u1bar-Fx3(k1)/m)-kgain*(phi(k1)-pi/2);
    % movement of the vehicle
    sy(k1+1)=sy(k1)+v(k1)*dt*cos(phi(k1));
    sx(k1+1)=sx(k1)+v(k1)*dt*sin(phi(k1));
    k1=k1+1;
end
%  after changing the road conditions
kchange=k1;
while v(k1)>vmin && k1<2100
    if omega1(k1)<1.1*vmin/r    % blocking wheel
       omega1(k1)=0;
       lambda1(k1)=1;
    else
       lambda1(k1)=1-r/v(k1)*omega1(k1);
    end
    if omega3(k1)<1.1*vmin/r    % blocking wheel
       omega3(k1)=0;
       lambda3(k1)=1;
    else
       lambda3(k1)=1-r/v(k1)*omega3(k1);
    end
    % wheel 3 on dry asphalt
    mu3(k1)=c1Asphalt-exp(-c2Asphalt*lambda3(k1))-c3Asphalt*lambda3(k1);
    omega3(k1+1)=omega3(k1) + (m*r/J*g*mu3(k1) - m*r/J*u3(k1))*dt;
    Fx3(k1)=g*mu3(k1)*m;
    % wheel 1 on icy road
    mu1(k1)=c1Nass-exp(-c2Nass*lambda1(k1))-c3Nass*lambda1(k1);
    omega1(k1+1)=omega1(k1) + (m*r/J*g*mu1(k1) - m*r/J*u1(k1))*dt;
    Fx1(k1)=g*mu1(k1)*m;
    v(k1+1)=v(k1)-(2*Fx1(k1)+2*Fx3(k1))/mV*dt;
    phi(k1+1)=phi(k1)+carwidth/2/JV*2*(-Fx1(k1)+Fx3(k1))*dt;
    % controller
    u1(k1+1)=kdec*(u1bar-Fx1(k1)/m)+kgain*(phi(k1)-pi/2);
    u3(k1+1)=kdec*(u1bar-Fx3(k1)/m)-kgain*(phi(k1)-pi/2);
    % movement of the vehicle
    sy(k1+1)=sy(k1)+v(k1)*dt*cos(phi(k1));
    sx(k1+1)=sx(k1)+v(k1)*dt*sin(phi(k1));
    k1=k1+1;
end
lambda1(k1)=lambda1(k1-1);
mu1(k1)=mu1(k1-1);
Fx1(k1)=Fx1(k1-1);
lambda3(k1)=lambda3(k1-1);
mu3(k1)=mu3(k1-1);
Fx3(k1)=Fx3(k1-1);
Tend=k1*dt;
Time=[0:dt:Tend-dt];
%
figure(14)
subplot(4,1,[1,2])
plot(Time, omega1,'b', Time, omega3,'b--');
hold on
plot([kchange*dt kchange*dt],[0 60], 'k:');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Changing road conditions');
latexylabel('$$\omega_1, \omega_3 \;$$'); % in $$\frac{\rm rad}{\rm s}$$
rotateY;
xleer;
xticksempty([0 1 2 3 4 5 6]);
yticks([0 20 40 60]);
axis([0 Tend 0 60]);
hold off
%
subplot(4,1,[3 4])
plot(Time, v,'b');
hold on
plot([kchange*dt kchange*dt],[0 15], 'k:');
latexylabel('$$v \;$$'); %  in $$\frac{\rm m}{\rm s}$$
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend 0 15]);
xticks([0 1 2 3 4 5 6]);
yticks([0 5 10 15]);
hold off
%
figure(13)
plot(sy, sx, 'b');
hold on
latexylabel('$$s$$ in m');
latexxlabel('$$y_{\rm K}$$ in m');
rotateY;
xticks([-0.1 0 0.1 0.2 0.3 0.4]);
yticks([0 10 20 30 40]);
axis([-0.15 0.25 0 30]);
hold off
%
figure(15)
subplot(2,1,1)
plot(Time, lambda1, 'b', Time, lambda3, 'b--');
hold on
plot([kchange*dt kchange*dt],[0 0.2], 'k:');
latextitle('$${\tt CarBrakeManoeuvre:}$$ Nonlinear model, controlled braking force');
axis([0 Tend 0 0.2]);
latexxlabel('$$t$$ in s');
latexylabel('$$\lambda_1, \lambda_3$$');
rotateY;
axis([0 3.5 0 0.07]);
xticks([0 1 2 3 4]);
yticks([0 0.05 0.1]);
hold off


%%  EPS figures
%
epsfigc15('CarBrakeManoeuvre');
