%Simulation of observer for vehicle steering
%kja 070707

aminit;

% Generate control signal and plot it
Tf=6;t=0:0.01:Tf;
w=2;u=sin(w*t)-0.1*t;

% System model
gamma=0.5;
A=[0 1;0 0];
B=[gamma;1];
C=[1 0];
x0=[0;1;0;0];
w0=1;z0=0.7;

%   Observer gains
L=[2*z0*w0;w0^2];

%   Form augmented system that contains process and observer dynamics
AA=[A zeros(2,2);L*C A-L*C];
BB=[B;B];
CC=[C 0 0;0 0 C];

%   Simulate system
[y,x]=lsim(AA,BB,CC,0,u,t,x0);

% Compute out the "road"
ind = 1:length(t)-1;
refx = y(ind,1)*5; refy = t(ind)'*5;
tanx = y(ind+1,1) - y(ind,1); tany = t(ind+1)' - t(ind)';
perp = [tany ./ hypot(tanx, tany), -tanx ./ hypot(tanx, tany)];

% Plot the trajectory we were trying to follow
figure(1); subplot(221);

plot(refx + perp(:,1), refy + perp(:,2), 'b--', ...
     refx - perp(:,1), refy - perp(:,2), 'b--', ...
     refx, refy, 'r-', ...
     'LineWidth', AM_ref_linewidth);
amaxis([0 30 0 30]);
xlabel('x [m]'); ylabel('y [m]')
set(gca, 'DataAspectRatio', [1 1 1]);
amprint('steering-obs-road.eps');

%   Plotting
figure(2); clf;

subplot(431)
plot(t,y(:,1),'',t,y(:,2),'--','LineWidth', AM_data_linewidth);
amaxis([0 Tf 0 6.01]);
set(gca, 'YTick', [0 2 4 6]);
ylabel('y');

lgh = legend('Act', 'Est', 'Orientation', 'vertical', ...
  'Location', 'southeast');
legend(lgh, 'boxoff');

subplot(432)
plot(t,y(:,1)-y(:,2),'LineWidth', AM_data_linewidth);
amaxis([0 Tf -0.1 0.5]);
ylabel('e1');

subplot(434)
plot(t,x(:,2),'-',t,x(:,4),'--','LineWidth', AM_data_linewidth);
amaxis([0 Tf -1 2]);
ylabel('v');xlabel('t');

subplot(435)
plot(t,x(:,2)-x(:,4),'b-','LineWidth', AM_data_linewidth);
amaxis([0 Tf -0.1 1.1]);
ylabel('e2');xlabel('t');
amprint('steering-obs.eps');
