Ai=[0 1;0 0]
Bi=[1 0;0 1]
Qi=eye(2,2)
epsilon=0.5;
Pi=are(Ai,2*epsilon*Bi*Bi',Qi)

Fi=[1 2;1.5 4]

tt=Pi*Bi*Fi
eig(tt)

N=4;
m=2;

Lg=[1 -1 0 0;-1 3 -1 -1;0 -1 1 0;0 -1 0 1]
X0=[-1 0.3 2 3 2.4 1.2 4 5];

d=[0.1 0.1;0.1 0.2;0.2 0.2;0.2 0.1];

%center_of_form=[1 0], initial_veh=center_of_form+d;
Xd0=[1 0 1 0 1 0 1 0];
X0=[-0.3 -0.4 0.2 -0.1 0.4 0.1 -0.5 0.2]; 
E0=X0-Xd0;
[t,y]=ode23(@def_track,[0 10],E0,[],N,m,Ai,Bi,Pi,Fi,epsilon,Lg,d);

e11=y(:,1);e12=y(:,2);
e21=y(:,3);e22=y(:,4);
e31=y(:,5);e32=y(:,6);
e41=y(:,7);e42=y(:,8);

xd11=cos(t)+d(1,1)*(-sin(t))+d(1,2)*cos(t);
xd12=sin(t)+d(1,1)*cos(t)+d(1,2)*sin(t);
xd21=cos(t)+d(2,1)*(-sin(t))+d(2,2)*cos(t);
xd22=sin(t)+d(2,1)*cos(t)+d(2,2)*sin(t);
xd31=cos(t)+d(3,1)*(-sin(t))+d(3,2)*cos(t);
xd32=sin(t)+d(3,1)*cos(t)+d(3,2)*sin(t);
xd41=cos(t)+d(4,1)*(-sin(t))+d(4,2)*cos(t);
xd42=sin(t)+d(4,1)*cos(t)+d(4,2)*sin(t);

x11=xd11+e11;x12=xd12+e12;
x21=xd21+e21;x22=xd22+e22;
x31=xd31+e31;x32=xd32+e32;
x41=xd41+e41;x42=xd42+e42;

plot(x11,x12,'r-',x21,x22,'b--',x31,x32,'g-.',x41,x42,'k:');
xlabel('X');
ylabel('Y');
%gtext('Start');
%gtext('Start');
%gtext('Start');
%gtext('Start');

hold on
for i=length(t)-15:5:length(t),
    plot(x11(i),x12(i),'ro',x21(i),x22(i),'bo',x31(i),x32(i),'go',x41(i),x42(i),'ko');
    t(i)
    hold on
end
hold off

