rungekutta4.m

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

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

function [T,Y,Theta]=RungeKutta4(t0,tmax,n,ar,al,vr,vl,b,theta0,XoY)

if(XoY=='y')
    f='(A*t+B)*sin(C*t.^2+D*t+theta0)';
else
    f='(A*t+B)*cos(C*t.^2+D*t+theta0)';   
end

y0=0;
h=(tmax-t0)/n;
m=length(y0);
T=[t0:h:t0+n*h]';Y=y0;
t=T(1);y=Y;
Theta=[theta0];
for i=1:n
   vr=ar*i+vr;
   vl=al*i+vl;
   
   A=(ar+al)/2;
   B=(vr+vl)/2;
   C=(ar-al)/(2*b);
   D=(vr-vl)/b; 

   theta=C*i^2+D*i+theta0;
   Theta=[Theta;theta];
   
   
   for j=1:m
      k1(j)=eval(f(j,:));
   end
   t=T(i)+h/2; y=Y(i,:)+h/2*k1;
   for j=1:m
      k2(j)=eval(f(j,:));
   end
   y=Y(i,:)+h/2*k2;
   for j=1:m
      k3(j)=eval(f(j,:));
   end
   t=T(i+1); y=Y(i,:)+h*k3;
   for j=1:m
      k4(j)=eval(f(j,:));
   end
   Y(i+1,:)=Y(i,:)+h/6*(k1+2*k2+2*k3+k4);
   y=Y(i+1,:);
end