% function x=XDOde4Step(funhnd,H,x0,u)
% 四階龍格庫塔算法
% funhnd 函數(shù)句柄
% H   仿真步長(zhǎng)
% x0  狀態(tài)
% u   控制

function x=XDOde4Step(funhnd,H,x0,u)

if nargin==3
% 不帶控制u時(shí)
  k1=feval(funhnd,x0);
  k2=feval(funhnd,x0+k1*H/2);
  k3=feval(funhnd,x0+k2*H/2);
  k4=feval(funhnd,x0+k3*H);
elseif nargin==4
% 帶控制u時(shí)
  k1=feval(funhnd,x0,u);
  k2=feval(funhnd,x0+k1*H/2,u);
  k3=feval(funhnd,x0+k2*H/2,u);
  k4=feval(funhnd,x0+k3*H,u);    
end
  x=x0+(k1+2*k2+2*k3+k4)*H/6;