% plotting script for car data;

k = 1;
axis([-10 10 -10 10])
axis manual

axis equal
hold on

w = 2;
xa = simout(:,1);
xb = simout(:,3);

ya = simout(:,2);
yb = simout(:,4);

xrr = xa + w/2*cos(psi - pi/2);
yrr = ya + w/2*sin(psi - pi/2);

xrf = xb + w/2*cos(psi - pi/2);
yrf = yb + w/2*sin(psi - pi/2);

xlf = xb + w/2*cos(psi + pi/2);
ylf = yb + w/2*sin(psi + pi/2);

xlr = xa + w/2*cos(psi + pi/2);
ylr = ya + w/2*sin(psi + pi/2);

while k <=length(simout)
%     if k > 1
%     plot([simout(k-1,1) simout(k-1,3) simout(k-1,5) simout(k-1,7) simout(k-1,1)],...
%         [simout(k-1,2) simout(k-1,4) simout(k-1,6) simout(k-1,8), simout(k-1,2)],'w-')
% end
    plot([xrr(k) xrf(k) xlf(k) xlr(k) xrr(k)],[yrr(k) yrf(k) ylf(k) ylr(k) yrr(k)],'b-')
%     plot([simout(k,1) simout(k,3)],...% simout(k,5) simout(k,7) simout(k,1)],...
%         [simout(k,2) simout(k,4)],'b-')% simout(k,6) simout(k,8), simout(k,2)],'b-')
M(:,k) = getframe;

k = k + 1;
end