%   kja 070704
aminit;					% initialize MATLAB

a=0.5;
gam=0.5;
pol=[1 2*a a^2];
Fm=tf(a^2,pol);
den=conv([gam 1],pol);
Fu=tf([a^2 0 0],den);
t=0:0.01:10;
y=4*step(Fm,t) - 2;
uff=4*step(Fu,t);

% Overhead view
figure(1); clf; subplot(221);
plot(3*t, y, 'LineWidth', AM_data_linewidth); hold on;
amaxis([-5 35 -10 10]);
set(gca, 'DataAspectRatio', [1.5 1.5 1]);

plot([-5 35], [-4 -4], 'k-', 'LineWidth', AM_ref_linewidth);
plot([-5 35], [0 0], 'k--', 'LineWidth', AM_ref_linewidth);
plot([-5 35], [4 4], 'k-', 'LineWidth', AM_ref_linewidth);

% Plot a small picture of the car (crude for now)
q0 = [0 -2]; qf = [30 2];
bodyx = [-0.5 2.5 2.5 -0.5 -0.5];
bodyy = [-1, -1, 1, 1, -1];
plot(bodyx + q0(1), bodyy + q0(2), 'r', 'LineWidth', AM_ref_linewidth);
plot(bodyx + qf(1), bodyy + qf(2), 'r', 'LineWidth', AM_ref_linewidth);
axis off; axis image;

amprint('steering-lanechange.eps');

figure(2); clf;
%   Plotting
subplot(421);
pl=plot(t,y);
set(pl,'Linewidth',AM_data_linewidth);
amaxis([0 10 -5 5])
ylabel('y');

subplot(423);
pl=plot(t,uff);
set(pl,'Linewidth',AM_data_linewidth);
amaxis([0 10 -1 1])
ylabel('u');xlabel('t');

amprint('steering-feedforward.eps');
