function d=d(kp,xi_k,i,u_vw)
d=[];
% sample time
dt = 1;
% desired velocity
dv = 10;
% linear path
lp = linearpath(kp);
% desired path
n = size(u_vw,1);
global k;
if isempty(k)
k=[xi_k,0];
end
dis = dv*dt*(i-k(4)-1);
global trigger1;
if isempty(trigger1)
trigger1 = 0;
end
trigger = 0;
idx = find(lp>dis,1);
jp = find(lp>dis+dt*dv,1)-idx;
if trigger1 == 0
if isempty(idx)
xd = kp(end,1);
yd = kp(end,2);
thetad = kp(end,3);
elseif isempty(jp)
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
elseif jp == 0
if idx == 1
dis = dis + dt*dv;
l = dis;
L = lp(1);
xd = (l/L)*kp(2,1);
yd = (l/L)*kp(2,2);
thetad = (l/L)*kp(2,3);
else
dis = dis + dt*dv;
l = dis-lp(idx-1);
L = lp(idx)-lp(idx-1);
xd = (1-l/L)*kp(idx,1)+(l/L)*kp(idx+1,1);
yd = (1-l/L)*kp(idx,2)+(l/L)*kp(idx+1,2);
thetad = (1-l/L)*kp(idx,3)+(l/L)*kp(idx+1,3);
end
else
if (xi_k-kp(idx+1,:))*diag([1,1,50])*(xi_k-kp(idx+1,:))' > .5
trigger1 = 1;
trigger = 1;
dis = lp(idx);
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
if sum(xi_k~=k(1:3))
k(4) = k(4) + 1;
end
else
if jp == 1
trigger = 0;
dis = dis + dt*dv;
idx = idx+1;
l = dis-lp(idx-1);
L = lp(idx)-lp(idx-1);
xd = (1-l/L)*kp(idx,1)+(l/L)*kp(idx+1,1);
yd = (1-l/L)*kp(idx,2)+(l/L)*kp(idx+1,2);
thetad = (1-l/L)*kp(idx,3)+(l/L)*kp(idx+1,3);
else
idx = idx+1;
dis = lp(idx);
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
end
end
end
else
if (xi_k-kp(idx+1,:))*diag([1,1,50])*(xi_k-kp(idx+1,:))' > .5
trigger = 1;
dis = lp(idx);
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
if sum(xi_k~=k(1:3))
k(4) = k(4) + 1;
end
else
if jp == 1
trigger1 = 0;
trigger = 0;
dis = dis + dt*dv;
idx = idx+1;
l = dis-lp(idx-1);
L = lp(idx)-lp(idx-1);
xd = (1-l/L)*kp(idx,1)+(l/L)*kp(idx+1,1);
yd = (1-l/L)*kp(idx,2)+(l/L)*kp(idx+1,2);
thetad = (1-l/L)*kp(idx,3)+(l/L)*kp(idx+1,3);
else
idx = idx+1;
dis = lp(idx);
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
end
end
end
d=[d;[xd yd,thetad]];
for j=2:n
xi_k1 = xi_k + (B(xi_k,u_vw(j,2))*(u_vw(j,1)))';
jp = find(lp>dis+dt*dv,1)-idx;
if trigger == 0
if isempty(idx)
xd = kp(end,1);
yd = kp(end,2);
thetad = kp(end,3);
elseif isempty(jp)
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
elseif jp == 0
if idx == 1
dis = dis + dt*dv;
l = dis;
L = lp(1);
xd = (l/L)*kp(2,1);
yd = (l/L)*kp(2,2);
thetad = (l/L)*kp(2,3);
else
dis = dis + dt*dv;
l = dis-lp(idx-1);
L = lp(idx)-lp(idx-1);
xd = (1-l/L)*kp(idx,1)+(l/L)*kp(idx+1,1);
yd = (1-l/L)*kp(idx,2)+(l/L)*kp(idx+1,2);
thetad = (1-l/L)*kp(idx,3)+(l/L)*kp(idx+1,3);
end
else
if (xi_k1-kp(idx+1,:))*diag([1,1,50])*(xi_k1-kp(idx+1,:))' > .5
trigger = 1;
dis = lp(idx);
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
else
if jp == 1
trigger = 0;
dis = dis + dt*dv;
idx = idx+1;
l = dis-lp(idx-1);
L = lp(idx)-lp(idx-1);
xd = (1-l/L)*kp(idx,1)+(l/L)*kp(idx+1,1);
yd = (1-l/L)*kp(idx,2)+(l/L)*kp(idx+1,2);
thetad = (1-l/L)*kp(idx,3)+(l/L)*kp(idx+1,3);
else
idx = idx+1;
dis = lp(idx);
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
end
end
end
else
if (xi_k1-kp(idx+1,:))*diag([1,1,50])*(xi_k1-kp(idx+1,:))' > .5
trigger = 1;
dis = lp(idx);
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
else
if jp == 1
trigger = 0;
dis = dis + dt*dv;
idx = idx+1;
l = dis-lp(idx-1);
L = lp(idx)-lp(idx-1);
xd = (1-l/L)*kp(idx,1)+(l/L)*kp(idx+1,1);
yd = (1-l/L)*kp(idx,2)+(l/L)*kp(idx+1,2);
thetad = (1-l/L)*kp(idx,3)+(l/L)*kp(idx+1,3);
else
idx = idx+1;
dis = lp(idx);
xd = kp(idx+1,1);
yd = kp(idx+1,2);
thetad = kp(idx+1,3);
end
end
end
d=[d;[xd yd,thetad]];
end
if sum(xi_k~=k(1:3))
k(1)=xi_k(1);
k(2)=xi_k(2);
k(3)=xi_k(3);
end