function [tout, xout] = rk4(xdotfun, t0, tfinal, x0, h, trace)

% Simple, fixed step size, Fourth order Runge-Kutta
% numerical integration of a system of first order
% ordinary differential equations.
% Modeling and Simulation of Dynamic Systems
% Robert L. Woods & Kent L. Lawrence

% Inputs:
%   xdotfun - A string containing the name of the m-file
%             which defines the system variable 
%             derivatives, xdot = f(x,t).
%   t0      - Initial value of time (independent
%             variable).
%   tfinal  - Final time.
%   x0      - Vector of initial values for the dependent
%             variables x.
%   h       - Fixed integration step size.
%   trace   - If trace is 1, intermediate values printed
%             from rk4.m. If trace is 0 or not specified,
%             values are not printed in rk4.m.
% Outputs:
%   tout    - Vector of time points at which solution is
%             tabulated.
%   xout    - Array of calculated values. Each column
%             is the time history of a dependent
%             variable.
% See example 9.6 for a sample of use.
% Watch for use of the transpose operator, '.

% Initialize variables
  t = t0;
  x = x0;
  tout = t;
  xout = x0';

  if nargin < 6
    trace = 0;
  end

% Set aside storage
  A = zeros(1)*x;
  B = zeros(1)*x;
  C = zeros(1)*x;
  D = zeros(1)*x;

  h2 = h/2;

  if trace
    clc, t, x
  end

% Integration loop
  while(t < tfinal)

    if t + h > tfinal
      h = tfinal - t;
    end

    A = feval(xdotfun, t, x)';
    B = feval(xdotfun, t+h2, x+h2*A)';
    C = feval(xdotfun, t+h2, x+h2*B)';
    D = feval(xdotfun, t+h, x+h*C)';

    t = t + h;
    x = x + h*(A + 2*B + 2*C + D)/6;

    tout = [tout; t];
    xout = [xout; x'];

    if trace
      clc, t, x
    end

  end;


