%Simulation of vehicle on curvy roadn
%   kja 070719

aminit;

t=0:0.1:50;
gamma=0.5;
w1=0.08;
%wo=0.3;zo=0.7;
wo=1;zo=0.707;
%wc=0.7;zc=0.9;
wc=1;zc=0.707;
yr=3*(sin(w1*t)+sin(2*w1*t));

A=[0 1;0 0];B=[gamma;1];C=[1 0];D=0;
ZC=zeros(size(C));
k1=wc^2;k2=2*zc*wc-gamma*wc^2;kr=k1;
l1=2*zo*wo;l2=wo^2;
%   Test of gain calculations
pc=[-zc*wc+i*wc*sqrt(1-zc^2) -zc*wc-i*wc*sqrt(1-zc^2)];
po=[-zo*wo+i*wo*sqrt(1-zo^2) -zo*wo-i*wo*sqrt(1-zo^2)]';
K=acker(A,B,pc);
L=acker(A',C',po);L=L';
%   Augmented system
xa=[-0.5;0.5;0;0];
Aa=[A -B*K;L*C A-B*K-L*C];
Ba=[B;B]*kr;
Ca=[C ZC;ZC C];
[y,x]=lsim(Aa,Ba,Ca,0,yr,t,xa);
u=-K*x(:,1:2)'+kr*yr;
ue=-K*x(:,3:4)'+kr*yr;

% Position and estimate, with lane boundaries
figure(1); clf; subplot(221); hold on;
h3 = plot(t, yr, 'k-.', 'Linewidth', AM_ref_linewidth);
h1 = plot(t, y(:,1), 'b-', 'Linewidth', AM_data_linewidth);
h2 = plot(t, y(:,2), 'r--', 'Linewidth', AM_thick_linewidth);
plot(t, yr+2, 'k:',t, yr-2, 'k:', 'LineWidth', AM_data_linewidth);
amaxis([0 15 -2 8]); box on;
xlabel('t'); ylabel('x');
lgh = legend(gca, [h1, h2, h3], 'State feedback', 'Output feedback', ...
  'Reference',  'Location', 'southeast');
legend(lgh, 'boxoff');

amprint('steering-sim-1.eps');

% PLot the velocity and inputs
figure(2); clf;

subplot(422); hold on;
plot(t, x(:,2), 'b-', 'Linewidth', AM_data_linewidth);
plot(t, x(:,4), 'r--', 'Linewidth', AM_thick_linewidth);
amaxis([0 15 -1 1]); box on; 
ylabel('v');

subplot(424); hold on;
plot(t, u, 'b-', 'Linewidth', AM_data_linewidth);
plot(t, ue, 'r--', 'Linewidth', AM_thick_linewidth);
amaxis([0 15 -1 1]); box on; 
xlabel('t'); ylabel('u');
% lgh = legend('State feedback', 'Output feedback', ...
%   'Orientation', 'horizontal', 'Location', 'northeast');
% legend(lgh, 'boxoff');

amprint('steering-sim-2.eps');