EOM1.m

function dX_dt = EOM1(t, X)
  global indexes

  r  = X(indexes.r);
  v  = X(indexes.v);

  r2 = dot(r,r);
  rm = sqrt(r2);
  r3 = r2 * rm;

  rdot  = v;
  vdot  = - r / r3;

  dX_dt = [ rdot' vdot' ]';
end

EOM1JAC.m

function dA_dt = EOM1JAC(t, A, varargin)
  global indexes

  r  = A(indexes.r);
  v  = A(indexes.v);
  J  = reshape(A(7:42,end), 6, 6);

  r2 = dot(r,r);
  rm = sqrt(r2);
  r3 = r2 * rm;
  r5 = r2 * r3;

  % propagate the states

  rdot  = v;
  vdot  = - r / r3;

  dX_dt = [ rdot' vdot' ]';

  % propagate the jacobian

  dR_dR = zeros(3,3);
  dR_dV = eye(3,3);
  dV_dR = 3/r5*r*r' - eye(3,3)/r3;
  dV_dV = zeros(3,3);

  dJ_dt = [ dR_dR dR_dV; dV_dR dV_dV ] * J;

  % assemble the vector of differentials

  dA_dt = [
    dX_dt
    dJ_dt(:) % ordered by columns
    ];
end

integrated_jacobians.m

close all; clear classes; clear all; clc;
format longG;
format compact;

global indexes tol

indexes.r = 1:3;
indexes.v = 4:6;

r0 = [ 1 0.2 0.1 ]';
v0 = [ 0.1 1 0.2 ]';
tf = 20;

x0 = [ r0' v0' ];

tol = 1e-9;

tic
[rf, vf, stm] = stm2(1.0, tf, r0, v0);
toc

det_stm = det(stm)

tic
J1 = jacobianest(@(x) propagate(x, tf), x0);
toc

det_est = det(J1)

tic
J2 = jacobian_ode45(x0, tf);
toc

det_ode45 = det(J2)

tic
J3 = jacobian_bv78(x0, tf);
toc

det_bv78 = det(J3)

tic
J4 = jacobian_rkf78(x0, tf);
toc

det_rkf78 = det(J4)

tic
J5 = jacobian_matlab(x0, tf);
toc

det_matlab = det(J5)

diff_est = (J1 - stm)
diff_ode45 = (J2 - stm)
diff_bv78 = (J3 - stm)
diff_rkf78 = (J4 - stm)
diff_matlab = (J5 - stm)

stm = stm
J1 = J1
J2 = J2
J3 = J3
J4 = J4
J5 = J5

function J = jacobian_matlab(x0, tf)
  global indexes tol

  finDiffOpts.DiffMinChange = 0;
  finDiffOpts.DiffMaxChange = inf;
  finDiffOpts.TypicalX = ones(length(x0),1);
  finDiffOpts.FinDiffType = 'forward';
  finDiffOpts.GradObj = 'off';
  finDiffOpts.GradConstr = 'off';
  finDiffOpts.UseParallel = false;
  finDiffOpts.FinDiffRelStep = sqrt(eps);
  finDiffOpts.Hessian = [];

  sizes.nVar = length(x0);
  sizes.mNonlinIneq = 0;
  sizes.mNonlinEq = 0;
  sizes.xShape = size(x0);

  finDiffFlags.fwdFinDiff = true;
  finDiffFlags.scaleObjConstr = false;
  finDiffFlags.chkComplexObj = false;
  finDiffFlags.isGrad = false;
  finDiffFlags.hasLBs = false(sizes.nVar,1);
  finDiffFlags.hasUBs = false(sizes.nVar,1);
  finDiffFlags.chkFunEval = false;
  finDiffFlags.sparseFinDiff = false;

  funfcn = optimfcnchk(@(x) propagate(x, tf),'fminunc',0,false,false,false)
  f = feval(funfcn{3},x0)
  J = sfdnls(x0,f,[],[],[],funfcn,[],[],finDiffOpts,sizes,finDiffFlags)
end

function J = jacobian_ode45(x0, tf)
  global indexes tol

  J0 = eye(6,6);
  A0 = [ x0(1:6) reshape(J0, 1, []) ];

  options = odeset('RelTol',tol,'AbsTol',tol);
  sol = ode45(@EOM1JAC, [0 tf], A0, options);
  xf = sol.y(1:6,end);
  J = reshape(sol.y(7:42,end), 6, 6);
end

function J = jacobian_bv78(x0, tf)
  global indexes tol

  J0 = eye(6,6);
  A0 = [ x0(1:6) reshape(J0, 1, []) ];

  re = tol;
  ae = tol;
  vectorized = false;
  [t, y] = BV78(@EOM1JAC, [0 tf], A0, re, ae, vectorized);
  y = y';
  xf = y(1:6,end);
  J = reshape(y(7:42,end), 6, 6);
end

function J = jacobian_rkf78(x0, tf)
  global indexes rkcoef tol

  J0 = eye(6,6);
  A0 = [ x0(1:6) reshape(J0, 1, []) ];

  rkcoef = 1;
  y = rkf78(@(t,x) EOM1JAC(t, x), 42, 0, tf, 1e-5, tol, A0');
  xf = y(1:6,end);
  J = reshape(y(7:42,end), 6, 6);
end

function xf = propagate(x0, tf)
  global indexes tol

  if tf == 0
    xf = x0(1:6);
  else
%    options = odeset('RelTol',tol,'AbsTol',tol);
    options = odeset();
    sol = ode45(@EOM1, [0 tf], x0(1:6), options);
    xf = sol.y(:,end);
  end
end

jacobianest.m

function [jac,err] = jacobianest(fun,x0)
% gradest: estimate of the Jacobian matrix of a vector valued function of n variables
% usage: [jac,err] = jacobianest(fun,x0)
%
%
% arguments: (input)
%  fun - (vector valued) analytical function to differentiate.
%        fun must be a function of the vector or array x0.
%
%  x0  - vector location at which to differentiate fun
%        If x0 is an nxm array, then fun is assumed to be
%        a function of n*m variables.
%
%
% arguments: (output)
%  jac - array of first partial derivatives of fun.
%        Assuming that x0 is a vector of length p
%        and fun returns a vector of length n, then
%        jac will be an array of size (n,p)
%
%  err - vector of error estimates corresponding to
%        each partial derivative in jac.
%
%
% Example: (nonlinear least squares)
%  xdata = (0:.1:1)';
%  ydata = 1+2*exp(0.75*xdata);
%  fun = @(c) ((c(1)+c(2)*exp(c(3)*xdata)) - ydata).^2;
%
%  [jac,err] = jacobianest(fun,[1 1 1])
%
%  jac =
%           -2           -2            0
%      -2.1012      -2.3222     -0.23222
%      -2.2045      -2.6926     -0.53852
%      -2.3096      -3.1176     -0.93528
%      -2.4158      -3.6039      -1.4416
%      -2.5225      -4.1589      -2.0795
%       -2.629      -4.7904      -2.8742
%      -2.7343      -5.5063      -3.8544
%      -2.8374      -6.3147      -5.0518
%      -2.9369      -7.2237      -6.5013
%      -3.0314      -8.2403      -8.2403
%
%  err =
%   5.0134e-15   5.0134e-15            0
%   5.0134e-15            0   2.8211e-14
%   5.0134e-15   8.6834e-15   1.5804e-14
%            0     7.09e-15   3.8227e-13
%   5.0134e-15   5.0134e-15   7.5201e-15
%   5.0134e-15   1.0027e-14   2.9233e-14
%   5.0134e-15            0   6.0585e-13
%   5.0134e-15   1.0027e-14   7.2673e-13
%   5.0134e-15   1.0027e-14   3.0495e-13
%   5.0134e-15   1.0027e-14   3.1707e-14
%   5.0134e-15   2.0053e-14   1.4013e-12
%
%  (At [1 2 0.75], jac should be numerically zero)
%
%
% See also: derivest, gradient, gradest
%
%
% Author: John D'Errico
% e-mail: [email protected]
% Release: 1.0
% Release date: 3/6/2007

% get the length of x0 for the size of jac
nx = numel(x0);

MaxStep = 1e-2;
StepRatio = 2.0000001;

% was a string supplied?
if ischar(fun)
  fun = str2func(fun);
end

% get fun at the center point
f0 = fun(x0);
f0 = f0(:);
n = length(f0);
if n==0
  % empty begets empty
  jac = zeros(0,nx);
  err = jac;
  return
end

relativedelta = MaxStep*StepRatio .^(0:-1:-10);
nsteps = length(relativedelta);

% total number of derivatives we will need to take
jac = zeros(n,nx);
err = jac;
for i = 1:nx
  x0_i = x0(i);
  if x0_i ~= 0
    delta = x0_i*relativedelta;
  else
    delta = relativedelta;
  end

  % evaluate at each step, centered around x0_i
  % difference to give a second order estimate
  fdel = zeros(n,nsteps);
  for j = 1:nsteps
    fdif = fun(swapelement(x0,i,x0_i + delta(j))) - ...
      fun(swapelement(x0,i,x0_i - delta(j)));

    fdel(:,j) = fdif(:);
  end

  % these are pure second order estimates of the
  % first derivative, for each trial delta.
  derest = fdel.*repmat(0.5 ./ delta,n,1);

  % The error term on these estimates has a second order
  % component, but also some 4th and 6th order terms in it.
  % Use Romberg exrapolation to improve the estimates to
  % 6th order, as well as to provide the error estimate.

  % loop here, as rombextrap coupled with the trimming
  % will get complicated otherwise.
  for j = 1:n
    [der_romb,errest] = rombextrap(StepRatio,derest(j,:),[2 4]);

    % trim off 3 estimates at each end of the scale
    nest = length(der_romb);
    trim = [1:3, nest+(-2:0)];
    [der_romb,tags] = sort(der_romb);
    der_romb(trim) = [];
    tags(trim) = [];

    errest = errest(tags);

    % now pick the estimate with the lowest predicted error
    [err(j,i),ind] = min(errest);
    jac(j,i) = der_romb(ind);
  end
end

end % mainline function end

% =======================================
%      sub-functions
% =======================================
function vec = swapelement(vec,ind,val)
% swaps val as element ind, into the vector vec
vec(ind) = val;

end % sub-function end

% ============================================
% subfunction - romberg extrapolation
% ============================================
function [der_romb,errest] = rombextrap(StepRatio,der_init,rombexpon)
% do romberg extrapolation for each estimate
%
%  StepRatio - Ratio decrease in step
%  der_init - initial derivative estimates
%  rombexpon - higher order terms to cancel using the romberg step
%
%  der_romb - derivative estimates returned
%  errest - error estimates
%  amp - noise amplification factor due to the romberg step

srinv = 1/StepRatio;

% do nothing if no romberg terms
nexpon = length(rombexpon);
rmat = ones(nexpon+2,nexpon+1);
% two romberg terms
rmat(2,2:3) = srinv.^rombexpon;
rmat(3,2:3) = srinv.^(2*rombexpon);
rmat(4,2:3) = srinv.^(3*rombexpon);

% qr factorization used for the extrapolation as well
% as the uncertainty estimates
[qromb,rromb] = qr(rmat,0);

% the noise amplification is further amplified by the Romberg step.
% amp = cond(rromb);

% this does the extrapolation to a zero step size.
ne = length(der_init);
rhs = vec2mat(der_init,nexpon+2,ne - (nexpon+2));
rombcoefs = rromb\(qromb'*rhs);
der_romb = rombcoefs(1,:)';

% uncertainty estimate of derivative prediction
s = sqrt(sum((rhs - rmat*rombcoefs).^2,1));
rinv = rromb\eye(nexpon+1);
cov1 = sum(rinv.^2,2); % 1 spare dof
errest = s'*12.7062047361747*sqrt(cov1(1));

end % rombextrap


% ============================================
% subfunction - vec2mat
% ============================================
function mat = vec2mat(vec,n,m)
% forms the matrix M, such that M(i,j) = vec(i+j-1)
[i,j] = ndgrid(1:n,0:m-1);
ind = i+j;
mat = vec(ind);
if n==1
  mat = mat';
end

end % vec2mat

rkf78.m

function xout = rkf78 (deq, neq, ti, tf, h, tetol, x)

% solve first order system of differential equations

% Runge-Kutta-Fehlberg 7(8) method

% input

%  deq   = name of function which defines the
%          system of differential equations
%  neq   = number of differential equations
%  ti    = initial simulation time
%  tf    = final simulation time
%  h     = initial guess for integration step size
%  tetol = truncation error tolerance (non-dimensional)
%  x     = integration vector at time = ti

% output

%  xout  = integration vector at time = tf

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global rkcoef ch alph beta

if (rkcoef == 1)

   % allocate arrays

   ch = zeros(13, 1);
   alph = zeros(13, 1);
   beta = zeros(13, 12);

   % define integration coefficients

   ch(6) = 34 / 105;
   ch(7) = 9 / 35;
   ch(8) = ch(7);
   ch(9) = 9 / 280;
   ch(10) = ch(9);
   ch(12) = 41 / 840;
   ch(13) = ch(12);

   alph(2) = 2 / 27;
   alph(3) = 1 / 9;
   alph(4) = 1 / 6;
   alph(5) = 5 / 12;
   alph(6) = 0.5;
   alph(7) = 5 / 6;
   alph(8) = 1 / 6;
   alph(9) = 2 / 3;
   alph(10) = 1 / 3;
   alph(11) = 1;
   alph(13) = 1;

   beta(2, 1) = 2 / 27;
   beta(3, 1) = 1 / 36;
   beta(4, 1) = 1 / 24;
   beta(5, 1) = 5 / 12;
   beta(6, 1) = 0.05;
   beta(7, 1) = -25 / 108;
   beta(8, 1) = 31 / 300;
   beta(9, 1) = 2;
   beta(10, 1) = -91 / 108;
   beta(11, 1) = 2383 / 4100;
   beta(12, 1) = 3 / 205;
   beta(13, 1) = -1777 / 4100;
   beta(3, 2) = 1 / 12;
   beta(4, 3) = 1 / 8;
   beta(5, 3) = -25 / 16;
   beta(5, 4) = -beta(5, 3);
   beta(6, 4) = 0.25;
   beta(7, 4) = 125 / 108;
   beta(9, 4) = -53 / 6;
   beta(10, 4) = 23 / 108;
   beta(11, 4) = -341 / 164;
   beta(13, 4) = beta(11, 4);
   beta(6, 5) = 0.2;
   beta(7, 5) = -65 / 27;
   beta(8, 5) = 61 / 225;
   beta(9, 5) = 704 / 45;
   beta(10, 5) = -976 / 135;
   beta(11, 5) = 4496 / 1025;
   beta(13, 5) = beta(11, 5);
   beta(7, 6) = 125 / 54;
   beta(8, 6) = -2 / 9;
   beta(9, 6) = -107 / 9;
   beta(10, 6) = 311 / 54;
   beta(11, 6) = -301 / 82;
   beta(12, 6) = -6 / 41;
   beta(13, 6) = -289 / 82;
   beta(8, 7) = 13 / 900;
   beta(9, 7) = 67 / 90;
   beta(10, 7) = -19 / 60;
   beta(11, 7) = 2133 / 4100;
   beta(12, 7) = -3 / 205;
   beta(13, 7) = 2193 / 4100;
   beta(9, 8) = 3;
   beta(10, 8) = 17 / 6;
   beta(11, 8) = 45 / 82;
   beta(12, 8) = -3 / 41;
   beta(13, 8) = 51 / 82;
   beta(10, 9) = -1 / 12;
   beta(11, 9) = 45 / 164;
   beta(12, 9) = 3 / 41;
   beta(13, 9) = 33 / 164;
   beta(11, 10) = 18 / 41;
   beta(12, 10) = 6 / 41;
   beta(13, 10) = 12 / 41;
   beta(13, 12) = 1;

   % set initialization indicator

   rkcoef = 0;
end

f = zeros(neq, 13);

xdot = zeros(1, neq);

xwrk = zeros(1, neq);

% compute integration "direction"

sdt = sign(tf - ti);

dt = abs(h) * sdt;

while (1)

   % load "working" time and integration vector

   twrk = ti;

   xwrk = x;

   % check for last dt

   if (abs(dt) > abs(tf - ti))
      dt = tf - ti;
   end

   % check for end of integration period

   if (abs(ti - tf) < 0.00000001)
      xout = x;
      return;
   end

   % evaluate equations of motion

   xdot = feval(deq, ti, x);

   f(:, 1) = xdot';

   % compute solution

   for k = 2:1:13
       kk = k - 1;

       for i = 1:1:neq
           x(i) = xwrk(i) + dt * sum(beta(k, 1:kk).* f(i, 1:kk));
       end

       ti = twrk + alph(k) * dt;

       xdot = feval(deq, ti, x);

       f(:, k) = xdot';
   end

   xerr = tetol;

   for i = 1:1:neq

       x(i) = xwrk(i) + dt * sum(ch.* f(i, :)');

       % truncation error calculations

       ter = abs((f(i, 1) + f(i, 11) - f(i, 12) - f(i, 13)) * ch(12) * dt);
       tol = abs(x(i)) * tetol + tetol;
       tconst = ter / tol;

       if (tconst > xerr)
          xerr = tconst;
       end
   end

   % compute new step size

   dt = 0.8 * dt * (1 / xerr) ^ (1 / 8);

   if (xerr > 1)
      % reject current step
      ti = twrk;
      x = xwrk;
   else
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      % accept current step              %
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      % perform graphics, additional     %
      % calculations, etc. at this point %
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   end
end

stm2.m

function [rf, vf, stm] = stm2(mu, tau, ri, vi)

% two body state transition matrix

% Shepperd's method

% input

%  tau = propagation time interval (seconds)
%  ri  = initial eci position vector (kilometers)
%  vi  = initial eci velocity vector (kilometers/second)

% output

%  rf  = final eci position vector (kilometers)
%  vf  = final eci velocity vector (kilometers/second)
%  stm = state transition matrix

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rf = zeros(3, 1);

vf = zeros(3, 1);

stm = zeros(6, 6);

% convergence criterion

tol = 1.0e-6;

n0 = dot(ri, vi);

r0 = norm(ri);

beta = (2.0 * mu / r0) - dot(vi, vi);

u = 0;

if (beta ~= 0.0)

    umax = 1.0 / sqrt(abs(beta));

else

    umax = 1.0e24;

end

umin = -umax;

if (beta <= 0.0)

    delu = 0.0;

else

    p = 2.0 * pi * mu * beta ^ (-1.5);

    n = fix((1.0 / p) * (tau + 0.5 * p - 2 * n0 / beta));

    delu = 2.0 * n * pi * beta ^ (-2.5);

end

tsav = 1.0e99;

niter = 0;

% kepler iteration loop

while (1)

    niter = niter + 1;

    q = beta * u * u;

    q = q / (1.0 + q);

    u0 = 1.0 - 2 * q;

    u1 = 2.0 * u * (1 - q);

    % continued fraction iteration

    n = 0;
    l = 3;
    d = 15;
    k = -9;
    a = 1;
    b = 1;
    g = 1;

    while (1)

        gsav = g;

        k = -k;

        l = l + 2;

        d = d + 4 * l;

        n = n + (1 + k) * l;

        a = d / (d - n * a * q);

        b = (a - 1) * b;

        g = g + b;

        if (abs(g - gsav) < tol)

            break;

        end

    end

    uu = (16.0 / 15.0) * u1 * u1 * u1 * u1 * u1 * g + delu;

    u2 = 2.0 * u1 * u1;

    u1 = 2.0 * u0 * u1;

    u0 = 2.0 * u0 * u0 - 1.0;

    u3 = beta * uu + u1 * u2 / 3.0;

    r1 = r0 * u0 + n0 * u1 + mu * u2;

    t = r0 * u1 + n0 * u2 + mu * u3;

    dtdu = 4.0 * r1 * (1.0 - q);

    % check for time convergence

    if (abs(t - tsav) < tol)

        break;

    end

    usav = u;

    tsav = t;

    terr = tau - t;

    if (abs(terr) < abs(tau) * tol)

        break;

    end

    du = terr / dtdu;

    if (du < 0.0)

        umax = u;

        u = u + du;

        if (u < umin)

            u = 0.5 * (umin + umax);

        end

    else

        umin = u;

        u = u + du;

        if (u > umax)

            u = 0.5 * (umin + umax);

        end

    end

    % check for independent variable convergence

    if (abs(u - usav) < tol)

        break;

    end

    % check for more than 20 iterations

    if (niter > 20)

        fprintf('\n\nmore than 20 iterations in stm2\n\n');

        pause;

        return;

    end

end

fm = -mu * u2 / r0;

ggm = -mu * u2 / r1;

f = 1.0 + fm;

g = r0 * u1 + n0 * u2;

ff = -mu * u1 / (r0 * r1);

gg = 1.0 + ggm;

% compute final state vector

rx = ri;

vx = vi;

rf = f * ri + g * vi;

vf = ff * ri + gg * vi;

uu = g * u2 + 3 * mu * uu;

a0 = mu / r0^3;

a1 = mu / r1^3;

m(1, 1) = ff * (u0 / (r0 * r1) + 1.0 / (r0 * r0) + 1.0 / (r1 * r1));
m(1, 2) = (ff * u1 + (ggm / r1)) / r1;
m(1, 3) = ggm * u1 / r1;

m(2, 1) = -(ff * u1 + (fm / r0)) / r0;
m(2, 2) = -ff * u2;
m(2, 3) = -ggm * u2;

m(3, 1) = fm * u1 / r0;
m(3, 2) = fm * u2;
m(3, 3) = g * u2;

m(1, 1) = m(1, 1) - a0 * a1 * uu;
m(1, 3) = m(1, 3) - a1 * uu;
m(3, 1) = m(3, 1) - a0 * uu;
m(3, 3) = m(3, 3) - uu;

m

for i = 1:1:2

    x = 2.0 * i - 3.0;

    ib = 3 * (2 - i);

    for j = 1:1:2

        jb = 3 * (j - 1);

        for ii = 1:1:3

            t1 = rf(ii) * m(i, j) + vf(ii) * m(i+1, j);

            t2 = rf(ii) * m(i, j+1) + vf(ii) * m(i+1, j+1);

            for jj = 1:1:3

                stm(ii + ib, jj + jb) = x * (t1 * rx(jj) + t2 * vx(jj));

            end

        end

    end
end

stm

for i = 1:1:3

    j = i + 3;

    stm(i, i) = stm(i, i) + f;

    stm(i, j) = stm(i, j) + g;

    stm(j, i) = stm(j, i) + ff;

    stm(j, j) = stm(j, j) + gg;

end