dydttaylor.m

该程序用PID系统实现差动转向车辆控制

%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

function ydrift=dydtTaylor(vr,vl,theta0,t,b,Yd)
%%this function calculate the drift from the center line (x axis)
%%with Taylor approssimation
sr=vr*t;
sl=vl*t;
%% an essential assumption here is that the initial drift


if(t==0)
    ydrift=Yd(end);
else
    ydrift=(sr+sl)*(sr-sl+b*theta0)/(2*t*b);
end

end