d2ydt2taylor.m

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

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

function yrate=d2ydt2Taylor(vr,vl,t,b,Yd2)
%% This function calculate the drift rate from center line (x axis)
%% with Taylor approssimation
sr=vr*t;
sl=vl*t;
%% An essential assumption is here is that the initial drift rate
%% for the robot is 0
if(t==0)
    yrate=Yd2(end);
else
    yrate=(sr+sl)*(sr-sl)/(2*b*t^2);
end
end