%     ARRVehicle.m
%
%  ARR for the vehicle control example
%
%  Jan Lunze
%  29.7.2016
% version of 18.12.2016
% for GMSC: 24.11.2023
%
echo off
clear all
close all
%%  Vehicle model from RandomRobots.m
%
%  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  u=acceleration in N
                % time in seconds
                % distance in m
                % velocity in m/s,   
                %         hence output  v=velocity in m/s
%               PI velocity controller
kI=20;           % kI=5
kP=70;          % kP=10
%               Controlled vehicle
%               input = set-point for velocity controller
%               output = velocity
Ac=[-c/m-kP/m -kI/m; 1 0];
Bc=[kP/m; -1];
Cc=[1 0];
Dc=0;
na=2;            % dynamical order of vehicle with velocity controller
%
%
%  Vehicle with controller; output=[v; s]
A=[Ac zeros(na,1); Cc 0];
B=[Bc; 0];
C=[1 zeros(1,na); zeros(1,na) 1];
D=zeros(2,1);
%

%% Step response of the vehicle
%  input=velocity set-point
%  output=position
figure(1)
Tend=50;
T=0:0.05:Tend;
vehicle=ss(A, B, C, D);
[Y, T]=step(vehicle, T);
subplot(2,1,1);
plot(T, Y(:,1),'b');
latexylabel('$$v$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
%latexxlabel('$$t$$ in s');
xleer
yticks([0 0.5 1]);
xticksempty([0 20 40 60 80 100]);
axis([0 Tend 0 1.2]);
latextitle('Step response of the vehicle');
subplot(2,1,2)
plot(T, Y(:,2),'b');
latexylabel('$$s$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 20 40 60 80 100]);

%%   Moving vehicle
%
figure(2)
%    Vehicle without position
Aplus=A(1:2,1:2);
Bplus=B(1:2,1);
Cplus=C(1,1:2);
nplus=2;
Vehicle=ss(Aplus,Bplus,Cplus,0);
%
Tend=50;
dt=0.05;
kend=ceil(Tend/dt)+1;
Time=[0:dt:Tend];
u0=80/3.6;                      % 80 km/h
ubar=120/3.6;                   % 120 km/h
%
%  Input
k1=4/dt;
U=[];
U(1:k1)=u0*ones(k1,1);
for k1=k1:kend
    U(k1)=ubar;
end
x0=-inv(Aplus)*Bplus*u0;
Y=lsim(Vehicle,U,Time,x0);
%
subplot(6,1,[1,2]);
plot(Time,3.6*U,'b--',Time,3.6*Y,'b');
latexylabel('$$y,u$$ in $$\frac{\rm km}{\rm h}$$');
rotateY;
xleer
xticksempty([0 20 40 60 80 100]);
yticks([80 100 120]);
axis([0 Tend 70 130]);
%


%%  State observer of the reduced model
%
figure(2)
%  eigenvalues of the vehicle: -0.135 \pm 0.04
lambdaB1=-0.2;
lambdaB2=-0.25;
LT=place(Aplus', Cplus', [lambdaB1, lambdaB2]);
L=LT';
AObserver=Aplus-L*Cplus;
BObserver=[Bplus; L];        % inputs: [u y]
CObserver=-Cplus;           % output = residual
DObserver=1;
xObserver0=x0;
nObserver=2;
%  Vehicle with observer
AOverall=[Aplus zeros(nplus,nObserver); L*Cplus AObserver];
BOverall=[Bplus; Bplus];
COverall=[Cplus -Cplus];
DOverall=0;
VehicleObs=ss(AOverall,BOverall,COverall,DOverall);
%
%  Vehicle with observer
R=lsim(VehicleObs,U,Time,[x0;xObserver0]);
%
subplot(6,1,[3,4])
fill([0 Tend Tend 0 0],[-1 -1 1 1 -1],[0.92 0.92 0.92]);
hold on
plot(Time,3.6*R(:,1),'b--');
hold on
latexylabel('$$r$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
%axis([0 Tend 70 130]);
%yticks([80 100 120]);
xticks([0 20 40 60 80 100]);




%%  Residual for changed mass of the vehicle
%
figure(2)
m=2500;
Am=[-c/m-kP/m -kI/m; 1 0];      %  eigenvalues: -0.05 \pm 0.07
Bm=[kP/m; -1];
Cm=[1 0];
Dm=0;
%
AOverall=[Am zeros(nplus,nObserver); L*Cplus AObserver];
BOverall=[Bm; Bplus];
COverall=[Cm -Cplus];
DOverall=0;
VehicleObs=ss(AOverall,BOverall,COverall,DOverall);
%
%  Vehicle with observer
R=lsim(VehicleObs,U,Time,[x0;xObserver0]);
%
subplot(6,1,[3,4])
plot(Time,3.6*R(:,1),'b');
rotateY;
%latexxlabel('$$t$$ in s');
xleer
yticks([-5 0 5]);
xticks([0 20 40 60 80 100]);
axis([0 Tend -5 5]);





%%
epsfigc('ARRVehicle');
%epsbild14('ARRVehicle','ARRVehicle');
